CAngularObservationMesh.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/CAngularObservationMesh.h>
#include <mrpt/poses/CPoint3D.h>
#include <mrpt/utils/stl_serialization.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::obs;
using namespace mrpt::maps;
using namespace mrpt::math;
using namespace mrpt::opengl;
using namespace mrpt::utils;
using namespace mrpt::poses;
using namespace mrpt::math;

IMPLEMENTS_SERIALIZABLE(
        CAngularObservationMesh, CRenderizableDisplayList, mrpt::opengl)

void CAngularObservationMesh::addTriangle(
        const TPoint3D& p1, const TPoint3D& p2, const TPoint3D& p3) const
{
        const TPoint3D* arr[3] = {&p1, &p2, &p3};
        CSetOfTriangles::TTriangle t;
        for (size_t i = 0; i < 3; i++)
        {
                t.x[i] = arr[i]->x;
                t.y[i] = arr[i]->y;
                t.z[i] = arr[i]->z;
                t.r[i] = m_color.R * (1.f / 255);
                t.g[i] = m_color.G * (1.f / 255);
                t.b[i] = m_color.B * (1.f / 255);
                t.a[i] = m_color.A * (1.f / 255);
        }
        triangles.push_back(t);
        CRenderizableDisplayList::notifyChange();
}

void CAngularObservationMesh::updateMesh() const
{
        CRenderizableDisplayList::notifyChange();

        size_t numRows = scanSet.size();
        triangles.clear();
        if (numRows <= 1)
        {
                actualMesh.setSize(0, 0);
                validityMatrix.setSize(0, 0);
                meshUpToDate = true;
                return;
        }
        if (pitchBounds.size() != numRows && pitchBounds.size() != 2) return;
        size_t numCols = scanSet[0].scan.size();
        actualMesh.setSize(numRows, numCols);
        validityMatrix.setSize(numRows, numCols);
        double* pitchs = new double[numRows];
        if (pitchBounds.size() == 2)
        {
                double p1 = pitchBounds[0];
                double p2 = pitchBounds[1];
                for (size_t i = 0; i < numRows; i++)
                        pitchs[i] = p1 +
                                                (p2 - p1) * static_cast<double>(i) /
                                                        static_cast<double>(numRows - 1);
        }
        else
                for (size_t i = 0; i < numRows; i++) pitchs[i] = pitchBounds[i];
        const bool rToL = scanSet[0].rightToLeft;
        for (size_t i = 0; i < numRows; i++)
        {
                const std::vector<float>& scan = scanSet[i].scan;
                const std::vector<char> valid = scanSet[i].validRange;
                const double pitchIncr = scanSet[i].deltaPitch;
                const double aperture = scanSet[i].aperture;
                const CPose3D origin = scanSet[i].sensorPose;
                // This is not an error...
                for (size_t j = 0; j < numCols; j++)
                        if ((validityMatrix(i, j) = (valid[j] != 0)))
                        {
                                double pYaw = aperture * ((static_cast<double>(j) /
                                                                                   static_cast<double>(numCols - 1)) -
                                                                                  0.5);
                                // Without the pitch since it's already within each sensorPose:
                                actualMesh(i, j) =
                                        (origin +
                                         CPose3D(0, 0, 0, rToL ? pYaw : -pYaw, pitchIncr)) +
                                        CPoint3D(scan[j], 0, 0);
                        }
        }
        delete[] pitchs;
        triangles.reserve(2 * (numRows - 1) * (numCols - 1));
        for (size_t k = 0; k < numRows - 1; k++)
        {
                for (size_t j = 0; j < numCols - 1; j++)
                {
                        int b1 = validityMatrix(k, j) ? 1 : 0;
                        int b2 = validityMatrix(k, j + 1) ? 1 : 0;
                        int b3 = validityMatrix(k + 1, j) ? 1 : 0;
                        int b4 = validityMatrix(k + 1, j + 1) ? 1 : 0;
                        switch (b1 + b2 + b3 + b4)
                        {
                                case 0:
                                case 1:
                                case 2:
                                        break;
                                case 3:
                                        if (!b1)
                                                addTriangle(
                                                        actualMesh(k, j + 1), actualMesh(k + 1, j),
                                                        actualMesh(k + 1, j + 1));
                                        else if (!b2)
                                                addTriangle(
                                                        actualMesh(k, j), actualMesh(k + 1, j),
                                                        actualMesh(k + 1, j + 1));
                                        else if (!b3)
                                                addTriangle(
                                                        actualMesh(k, j), actualMesh(k, j + 1),
                                                        actualMesh(k + 1, j + 1));
                                        else if (!b4)
                                                addTriangle(
                                                        actualMesh(k, j), actualMesh(k, j + 1),
                                                        actualMesh(k + 1, j));
                                        break;
                                case 4:
                                        addTriangle(
                                                actualMesh(k, j), actualMesh(k, j + 1),
                                                actualMesh(k + 1, j));
                                        addTriangle(
                                                actualMesh(k + 1, j + 1), actualMesh(k, j + 1),
                                                actualMesh(k + 1, j));
                        }
                }
        }
        meshUpToDate = true;
}

void CAngularObservationMesh::render_dl() const
{
#if MRPT_HAS_OPENGL_GLUT
        if (mEnableTransparency)
        {
                glEnable(GL_BLEND);
                glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
        }
        else
        {
                glEnable(GL_DEPTH_TEST);
                glDisable(GL_BLEND);
        }
        glEnable(GL_COLOR_MATERIAL);
        glShadeModel(GL_SMOOTH);
        if (!meshUpToDate) updateMesh();
        if (!mWireframe) glBegin(GL_TRIANGLES);
        for (size_t i = 0; i < triangles.size(); i++)
        {
                const CSetOfTriangles::TTriangle& t = triangles[i];
                float ax = t.x[1] - t.x[0];
                float bx = t.x[2] - t.x[0];
                float ay = t.y[1] - t.y[0];
                float by = t.y[2] - t.y[0];
                float az = t.z[1] - t.z[0];
                float bz = t.z[2] - t.z[0];
                glNormal3f(ay * bz - az * by, az * bx - ax * bz, ax * by - ay * bx);
                if (mWireframe) glBegin(GL_LINE_LOOP);
                for (int i = 0; i < 3; i++)
                {
                        glColor4f(t.r[i], t.g[i], t.b[i], t.a[i]);
                        glVertex3f(t.x[i], t.y[i], t.z[i]);
                }
                if (mWireframe) glEnd();
        }
        if (!mWireframe) glEnd();
        glDisable(GL_BLEND);
        glDisable(GL_LIGHTING);
#endif
}

bool CAngularObservationMesh::traceRay(
        const mrpt::poses::CPose3D& o, double& dist) const
{
        MRPT_UNUSED_PARAM(o);
        MRPT_UNUSED_PARAM(dist);
        // TODO: redo
        return false;
}

bool CAngularObservationMesh::setScanSet(
        const std::vector<mrpt::obs::CObservation2DRangeScan>& scans)
{
        CRenderizableDisplayList::notifyChange();

        // Returns false if the scan is inconsistent
        if (scans.size() > 0)
        {
                size_t setSize = scans[0].scan.size();
                bool rToL = scans[0].rightToLeft;
                for (std::vector<CObservation2DRangeScan>::const_iterator it =
                                 scans.begin() + 1;
                         it != scans.end(); ++it)
                {
                        if (it->scan.size() != setSize) return false;
                        if (it->rightToLeft != rToL) return false;
                }
        }
        scanSet = scans;
        meshUpToDate = false;
        return true;
}

void CAngularObservationMesh::setPitchBounds(
        const double initial, const double final)
{
        CRenderizableDisplayList::notifyChange();

        pitchBounds.clear();
        pitchBounds.push_back(initial);
        pitchBounds.push_back(final);
        meshUpToDate = false;
}
void CAngularObservationMesh::setPitchBounds(const std::vector<double>& bounds)
{
        CRenderizableDisplayList::notifyChange();

        pitchBounds = bounds;
        meshUpToDate = false;
}
void CAngularObservationMesh::getPitchBounds(
        double& initial, double& final) const
{
        initial = pitchBounds.front();
        final = pitchBounds.back();
}
void CAngularObservationMesh::getPitchBounds(std::vector<double>& bounds) const
{
        bounds = pitchBounds;
}
void CAngularObservationMesh::getScanSet(
        std::vector<CObservation2DRangeScan>& scans) const
{
        scans = scanSet;
}

void CAngularObservationMesh::generateSetOfTriangles(
        CSetOfTriangles::Ptr& res) const
{
        if (!meshUpToDate) updateMesh();
        res->insertTriangles(triangles.begin(), triangles.end());
        // for (vector<CSetOfTriangles::TTriangle>::iterator
        // it=triangles.begin();it!=triangles.end();++it) res->insertTriangle(*it);
}

struct CAngularObservationMesh_fnctr
{
        CPointsMap* m;
        CAngularObservationMesh_fnctr(CPointsMap* p) : m(p) {}
        inline void operator()(const CObservation2DRangeScan& obj)
        {
                m->insertObservation(&obj);
        }
};

void CAngularObservationMesh::generatePointCloud(CPointsMap* out_map) const
{
        ASSERT_(out_map);
        out_map->clear();
        /*      size_t numRows=scanSet.size();
                if ((pitchBounds.size()!=numRows)&&(pitchBounds.size()!=2)) return;
                std::vector<double> pitchs(numRows);
                if (pitchBounds.size()==2)      {
                        double p1=pitchBounds[0];
                        double p2=pitchBounds[1];
                        for (size_t i=0;i<numRows;i++)
           pitchs[i]=p1+(p2-p1)*static_cast<double>(i)/static_cast<double>(numRows-1);
                }       else for (size_t i=0;i<numRows;i++) pitchs[i]=pitchBounds[i];
                for (size_t i=0;i<numRows;i++) out_map->insertObservation(&scanSet[i]);
        */

        std::for_each(
                scanSet.begin(), scanSet.end(), CAngularObservationMesh_fnctr(out_map));
}

/*---------------------------------------------------------------
   Implements the writing to a CStream capability of
         CSerializable objects
  ---------------------------------------------------------------*/
void CAngularObservationMesh::writeToStream(
        mrpt::utils::CStream& out, int* version) const
{
        if (version)
                *version = 0;
        else
        {
                writeToStreamRender(out);
                // Version 0:
                out << pitchBounds << scanSet << mWireframe << mEnableTransparency;
        }
}

/*---------------------------------------------------------------
        Implements the reading from a CStream capability of
                CSerializable objects
  ---------------------------------------------------------------*/
void CAngularObservationMesh::readFromStream(
        mrpt::utils::CStream& in, int version)
{
        switch (version)
        {
                case 0:
                        readFromStreamRender(in);
                        in >> pitchBounds >> scanSet >> mWireframe >> mEnableTransparency;
                        break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
        meshUpToDate = false;
}

void CAngularObservationMesh::TDoubleRange::values(
        std::vector<double>& vals) const
{
        double value = initialValue();
        double incr = increment();
        size_t am = amount();
        vals.resize(am);
        for (size_t i = 0; i < am - 1; i++, value += incr) vals[i] = value;
        vals[am - 1] = finalValue();
}

void CAngularObservationMesh::getTracedRays(CSetOfLines::Ptr& res) const
{
        if (!meshUpToDate) updateMesh();
        size_t count = 0;
        for (size_t i = 0; i < validityMatrix.getRowCount(); i++)
                for (size_t j = 0; j < validityMatrix.getColCount(); j++)
                        if (validityMatrix(i, j)) count++;
        res->reserve(count);
        for (size_t i = 0; i < actualMesh.getRowCount(); i++)
                for (size_t j = 0; j < actualMesh.getColCount(); j++)
                        if (validityMatrix(i, j))
                                res->appendLine(
                                        TPose3D(scanSet[i].sensorPose), actualMesh(i, j));
}

class FAddUntracedLines
{
   public:
        CSetOfLines::Ptr& lins;
        const CPoint3D& pDist;
        std::vector<double> pitchs;
        FAddUntracedLines(
                CSetOfLines::Ptr& l, const CPoint3D& p, const std::vector<double>& pi)
                : lins(l), pDist(p), pitchs()
        {
                pitchs.reserve(pi.size());
                for (std::vector<double>::const_reverse_iterator it = pi.rbegin();
                         it != pi.rend(); ++it)
                        pitchs.push_back(*it);
        }
        void operator()(const CObservation2DRangeScan& obs)
        {
                size_t hm = obs.scan.size();
                for (std::vector<char>::const_iterator it = obs.validRange.begin();
                         it != obs.validRange.end(); ++it)
                        if (*it) hm--;
                lins->reserve(hm);
                for (size_t i = 0; i < obs.scan.size(); i++)
                        if (!obs.validRange[i])
                        {
                                double yaw =
                                        obs.aperture * ((static_cast<double>(i) /
                                                                         static_cast<double>(obs.scan.size() - 1)) -
                                                                        0.5);
                                lins->appendLine(
                                        TPoint3D(obs.sensorPose),
                                        TPoint3D(
                                                obs.sensorPose +
                                                CPose3D(
                                                        0, 0, 0, obs.rightToLeft ? yaw : -yaw,
                                                        obs.deltaPitch * i + pitchs.back(), 0) +
                                                pDist));
                        }
                pitchs.pop_back();
        }
};
void CAngularObservationMesh::getUntracedRays(
        CSetOfLines::Ptr& res, double dist) const
{
        for_each(
                scanSet.begin(), scanSet.end(),
                FAddUntracedLines(res, dist, pitchBounds));
}

TPolygon3D createFromTriangle(const CSetOfTriangles::TTriangle& t)
{
        TPolygon3D res(3);
        for (size_t i = 0; i < 3; i++)
        {
                res[i].x = t.x[i];
                res[i].y = t.y[i];
                res[i].z = t.z[i];
        }
        return res;
}

void CAngularObservationMesh::generateSetOfTriangles(
        std::vector<TPolygon3D>& res) const
{
        if (!meshUpToDate) updateMesh();
        res.resize(triangles.size());
        transform(
                triangles.begin(), triangles.end(), res.begin(), createFromTriangle);
}

void CAngularObservationMesh::getBoundingBox(
        mrpt::math::TPoint3D& bb_min, mrpt::math::TPoint3D& bb_max) const
{
        if (!meshUpToDate) updateMesh();

        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 < triangles.size(); i++)
        {
                const CSetOfTriangles::TTriangle& t = triangles[i];

                keep_min(bb_min.x, t.x[0]);
                keep_max(bb_max.x, t.x[0]);
                keep_min(bb_min.y, t.y[0]);
                keep_max(bb_max.y, t.y[0]);
                keep_min(bb_min.z, t.z[0]);
                keep_max(bb_max.z, t.z[0]);

                keep_min(bb_min.x, t.x[1]);
                keep_max(bb_max.x, t.x[1]);
                keep_min(bb_min.y, t.y[1]);
                keep_max(bb_max.y, t.y[1]);
                keep_min(bb_min.z, t.z[1]);
                keep_max(bb_max.z, t.z[1]);

                keep_min(bb_min.x, t.x[2]);
                keep_max(bb_max.x, t.x[2]);
                keep_min(bb_min.y, t.y[2]);
                keep_max(bb_max.y, t.y[2]);
                keep_min(bb_min.z, t.z[2]);
                keep_max(bb_max.z, t.z[2]);
        }

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