CPlanarLaserScan.cpp

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

#include "maps-precomp.h" // Precomp header


#include <mrpt/opengl/CPlanarLaserScan.h>
#include <mrpt/utils/CStream.h>


#if MRPT_HAS_OPENGL_GLUT
        #ifdef MRPT_OS_WINDOWS
                // Windows:
                #include <windows.h>
        #endif

        #ifdef MRPT_OS_APPLE
                #include <OpenGL/gl.h>
        #else
                #include <GL/gl.h>
        #endif
#endif

// Include libraries in linking:
#if MRPT_HAS_OPENGL_GLUT && defined(MRPT_OS_WINDOWS)
                // WINDOWS:
                #if defined(_MSC_VER) || defined(__BORLANDC__)
                        #pragma comment (lib,"opengl32.lib")
                        #pragma comment (lib,"GlU32.lib")
                #endif
#endif // MRPT_HAS_OPENGL_GLUT


using namespace mrpt;
using namespace mrpt::opengl;
using namespace mrpt::utils;
using namespace mrpt::math;
using namespace std;

IMPLEMENTS_SERIALIZABLE( CPlanarLaserScan, CRenderizableDisplayList, mrpt::opengl )


/*---------------------------------------------------------------
                                Constructor
  ---------------------------------------------------------------*/
CPlanarLaserScan::CPlanarLaserScan() :
        m_scan(),
        m_cache_points(),
        m_cache_valid(false),
        m_line_width(1),
        m_line_R(1.f),m_line_G(0.f),m_line_B(0.f),m_line_A(0.5f),
        m_points_width(3),
        m_points_R(1.0f),m_points_G(0.0f),m_points_B(0.0f),m_points_A(1.0f),
        m_plane_R(0.01f),m_plane_G(0.01f),m_plane_B(0.6f),m_plane_A(0.6f),
        m_enable_points(true), m_enable_line(true), m_enable_surface(true)
{
}

/*---------------------------------------------------------------
                                                        clear
  ---------------------------------------------------------------*/
void CPlanarLaserScan::clear()
{
        CRenderizableDisplayList::notifyChange();
        m_scan.resizeScan(0);
}


/*---------------------------------------------------------------
                                                        render
  ---------------------------------------------------------------*/
void   CPlanarLaserScan::render_dl() const
{
#if MRPT_HAS_OPENGL_GLUT
        ASSERT_(m_scan.scan.size()==m_scan.validRange.size());

        // Load into cache:
        if (!m_cache_valid)
        {
                m_cache_valid=true;
                m_cache_points.clear();
                m_cache_points.insertionOptions.minDistBetweenLaserPoints = 0;
                m_cache_points.insertionOptions.isPlanarMap=false;

                m_cache_points.insertObservation( &m_scan );
        }

        size_t  i,n;
        const float     *x,*y,*z;

        m_cache_points.getPointsBuffer(n,x,y,z);
        if (!n || !x) return;

        glEnable(GL_BLEND);
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

        // LINES
        // ----------------------------
        if (n>1 && m_enable_line)
        {
                glLineWidth(m_line_width);      checkOpenGLError();

                glBegin( GL_LINES );
                glColor4f( m_line_R,m_line_G,m_line_B,m_line_A );

                for (i=0;i<n-1;i++)
                {
                        glVertex3f( x[i],y[i],z[i] );
                        glVertex3f( x[i+1],y[i+1],z[i+1] );
                }
                glEnd();
                checkOpenGLError();
        }

        // POINTS
        // ----------------------------
        if (n>0 && m_enable_points)
        {
                glPointSize(m_points_width);
                checkOpenGLError();

                glBegin( GL_POINTS );
                glColor4f( m_points_R,m_points_G,m_points_B,m_points_A );

                for (i=0;i<n;i++)
                {
                        glVertex3f( x[i],y[i],z[i] );
                }
                glEnd();
                checkOpenGLError();
        }

        // SURFACE:
        // ------------------------------
        if (n>1 && m_enable_surface)
        {
                glBegin( GL_TRIANGLES );

                glColor4f(m_plane_R,m_plane_G,m_plane_B,m_plane_A);

                for (i=0;i<n-1;i++)
                {
                        glVertex3f( m_scan.sensorPose.x(), m_scan.sensorPose.y(), m_scan.sensorPose.z() );
                        glVertex3f( x[i],y[i],z[i] );
                        glVertex3f( x[i+1],y[i+1],z[i+1] );
                }
                glEnd();
                checkOpenGLError();
        }

        glDisable(GL_BLEND);

#endif
}

/*---------------------------------------------------------------
   Implements the writing to a CStream capability of
     CSerializable objects
  ---------------------------------------------------------------*/
void  CPlanarLaserScan::writeToStream(mrpt::utils::CStream &out,int *version) const
{

        if (version)
                *version = 1;
        else
        {
                writeToStreamRender(out);
                out << m_scan;
                out << m_line_width
                        << m_line_R << m_line_G << m_line_B << m_line_A
                        << m_points_width
                        << m_points_R << m_points_G << m_points_B << m_points_A
                        << m_plane_R << m_plane_G << m_plane_B << m_plane_A
                        << m_enable_points << m_enable_line << m_enable_surface; // new in v1
        }
}

/*---------------------------------------------------------------
        Implements the reading from a CStream capability of
                CSerializable objects
  ---------------------------------------------------------------*/
void  CPlanarLaserScan::readFromStream(mrpt::utils::CStream &in,int version)
{
        switch(version)
        {
        case 0:
        case 1:
                {
                        readFromStreamRender(in);
                        in >> m_scan;
                        in >> m_line_width
                                >> m_line_R >> m_line_G >> m_line_B >> m_line_A
                                >> m_points_width
                                >> m_points_R >> m_points_G >> m_points_B >> m_points_A
                                >> m_plane_R >> m_plane_G >> m_plane_B >> m_plane_A;

            if (version>=1)
            {
                in >> m_enable_points >> m_enable_line >> m_enable_surface; // new in v1
            }
            else
            {
                m_enable_points = m_enable_line = m_enable_surface = true;
            }
                } break;
        default:
                MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)

        };
}


void CPlanarLaserScan::getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const
{
        // Load into cache:
        if (!m_cache_valid)
        {
                m_cache_valid=true;
                m_cache_points.clear();
                m_cache_points.insertionOptions.minDistBetweenLaserPoints = 0;
                m_cache_points.insertionOptions.isPlanarMap=false;

                m_cache_points.insertObservation( &m_scan );
        }

        size_t n;
        const float     *x,*y,*z;

        m_cache_points.getPointsBuffer(n,x,y,z);
        if (!n || !x) return;

        bb_min = mrpt::math::TPoint3D(std::numeric_limits<double>::max(),std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
        bb_max = mrpt::math::TPoint3D(-std::numeric_limits<double>::max(),-std::numeric_limits<double>::max(),-std::numeric_limits<double>::max());

        for (size_t i=0;i<n;i++)
        {
                keep_min(bb_min.x, x[i]);  keep_max(bb_max.x, x[i]);
                keep_min(bb_min.y, y[i]);  keep_max(bb_max.y, y[i]);
                keep_min(bb_min.z, z[i]);  keep_max(bb_max.z, z[i]);
        }

        // Convert to coordinates of my parent:
        m_pose.composePoint(bb_min, bb_min);
        m_pose.composePoint(bb_max, bb_max);
}