CHeightGridMap2D_Base.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, 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/maps/CHeightGridMap2D_Base.h>
#include <mrpt/math/geometry.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/obs/CObservationVelodyneScan.h>
#include <mrpt/maps/CSimplePointsMap.h>

using namespace mrpt::maps;
using namespace std;

CHeightGridMap2D_Base::TPointInsertParams::TPointInsertParams()
    : pt_z_std(0.0), update_map_after_insertion(true)
{
}

CHeightGridMap2D_Base::CHeightGridMap2D_Base() {}
CHeightGridMap2D_Base::~CHeightGridMap2D_Base() {}
bool CHeightGridMap2D_Base::getMinMaxHeight(float& z_min, float& z_max) const
{
    const size_t size_x = dem_get_size_x();
    const size_t size_y = dem_get_size_y();

    bool any = false;
    z_min = z_max = 0;
    for (size_t x = 0; x < size_x; x++)
        for (size_t y = 0; y < size_y; y++)
        {
            double z;
            if (dem_get_z_by_cell(x, y, z))
            {
                if (!any)
                {
                    // First:
                    any = true;
                    z_min = z_max = z;
                }
                else
                {
                    mrpt::keep_max(z_max, z);
                    mrpt::keep_min(z_min, z);
                }
            }
        }
    return any;
}

bool CHeightGridMap2D_Base::intersectLine3D(
    const mrpt::math::TLine3D& ray, mrpt::math::TObject3D& obj) const
{
    using namespace mrpt::math;

    MRPT_START

    obj = TObject3D();

    const double resolution = dem_get_resolution();
    float z_min, z_max;
    if (!getMinMaxHeight(z_min, z_max)) return false;

    // 1st: intersections with 2 horizontal planes at the grid Z limits:
    const TPlane horz_plane_above(
        TPoint3D(0, 0, z_max + 1), TPoint3D(1, 0, z_max + 1),
        TPoint3D(0, 1, z_max + 1));
    const TPlane horz_plane_below(
        TPoint3D(0, 0, z_min - 1), TPoint3D(1, 0, z_min - 1),
        TPoint3D(0, 1, z_min - 1));
    TPoint3D pt_ab, pt_be;
    {
        TObject3D int_ab, int_be;
        intersect(ray, horz_plane_above, int_ab);
        intersect(ray, horz_plane_below, int_be);

        if (!int_ab.getPoint(pt_ab) || !int_be.getPoint(pt_be)) return false;
    }

    // Now, go from pt_ab -> pt_be doing "ray-tracing" and find the collision
    // with a cell:
    TPoint3D pt = pt_ab;
    TPoint3D Apt = pt_be - pt_ab;
    const double totalDist = Apt.norm();
    if (totalDist == 0) return false;
    // The step:
    Apt *= resolution * 0.99 / totalDist;

    TPoint3D Apt_half = Apt;
    Apt_half *= 0.5;

    const size_t N = ceil(totalDist / resolution);

    for (size_t i = 0; i < N; i++)
    {
        // Mid point between this and next step:
        const TPoint3D testPt = pt + Apt_half;
        // get its height in the grid:
        double pt_z;
        if (dem_get_z(testPt.x, testPt.y, pt_z))
        {
            // Do we go thru the cell?
            if (pt_z >= std::min(pt.z, pt.z + Apt.z) &&
                pt_z < std::max(pt.z, pt.z + Apt.z))
            {
                // yes:
                TPoint3D colPt(testPt.x, testPt.y, pt_z);
                obj = TObject3D(colPt);
                return true;
            }
        }
        pt += Apt;
    }

    // No collision found!
    return false;

    // None found:
    MRPT_END
}

bool CHeightGridMap2D_Base::dem_internal_insertObservation(
    const mrpt::obs::CObservation* obs, const mrpt::poses::CPose3D* robotPose)
{
    using namespace mrpt::poses;
    using namespace mrpt::obs;

    MRPT_START

    CPose3D robotPose3D;  // Default: 0,0,0

    if (robotPose) robotPose3D = (*robotPose);

    // Points to insert:
    CSimplePointsMap thePointsMoved;

    if (IS_CLASS(obs, CObservation2DRangeScan))
    {
        /********************************************************************
                    OBSERVATION TYPE: CObservation2DRangeScan
        ********************************************************************/
        const CObservation2DRangeScan* o =
            static_cast<const CObservation2DRangeScan*>(obs);

        // Create points map, if not created yet:
        CPointsMap::TInsertionOptions opts;
        const CPointsMap* thePoints =
            o->buildAuxPointsMap<mrpt::maps::CPointsMap>(&opts);

        // And rotate to the robot pose:
        thePointsMoved.changeCoordinatesReference(*thePoints, robotPose3D);
    }
    else if (IS_CLASS(obs, CObservationVelodyneScan))
    {
        /********************************************************************
                    OBSERVATION TYPE: CObservationVelodyneScan
        ********************************************************************/
        const CObservationVelodyneScan* o =
            static_cast<const CObservationVelodyneScan*>(obs);

        // Create points map, if not created yet:
        thePointsMoved.loadFromVelodyneScan(*o, &robotPose3D);
    }

    // Factorized insertion of points, for different observation classes:
    if (!thePointsMoved.empty())
    {
        TPointInsertParams pt_params;
        pt_params.update_map_after_insertion =
            false;  // update only once at end

        const size_t N = thePointsMoved.size();
        for (size_t i = 0; i < N; i++)
        {
            float x, y, z;
            thePointsMoved.getPoint(i, x, y, z);
            insertIndividualPoint(x, y, z, pt_params);
        }  // end for i
        this->dem_update_map();
        return true;  // Done, new points inserted
    }
    return false;  // No insertion done
    MRPT_END
}