PlaneInferredInfo.cpp

Go to the documentation of this file.

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

/*  Plane-based Map (PbMap) library
 *  Construction of plane-based maps and localization in it from RGBD Images.
 *  Writen by Eduardo Fernandez-Moral. See docs for <a
 * href="group__mrpt__pbmap__grp.html" >mrpt-pbmap</a>
 */

#include "pbmap-precomp.h"  // Precompiled headers

#include <mrpt/pbmap/PlaneInferredInfo.h>

#if MRPT_HAS_PCL

using namespace std;
using namespace mrpt::pbmap;

/**!
 * Check if the input plane correpond to the floor. For that we assume that:
 * a) the vertical axis of the sensor's reference frame and the gravity axis are
 * simmilarly oriented,
 * b) the sensor is placed between 0.5 and 2 m above the floor plane
 * c) the input plane is big enough > 5m2.
 * d) ***we are not checking this now*** Most of the map planes are above the
 * floor plane
 */

bool PlaneInferredInfo::searchTheFloor(
    Eigen::Matrix4f& poseSensor, Plane& plane)
{
    if (plane.areaVoxels < 2.0) return false;

    // The angle between the Y axis of the camera and the normal of the plane
    // will normally be around: a) -1 for the floor b) 1 for the ceiling c) 0
    // for the walls
    double cosGravityDir = poseSensor.col(1).head(3).dot(plane.v3normal);
    if (cosGravityDir < -0.94)  // cos 20º = 0.9397
    {
        double sensorHeight =
            (plane.v3normal.dot(poseSensor.col(3).head(3) - plane.v3center));

        if (sensorHeight < 0.7 || sensorHeight > 2.0) return false;

        mPbMap.FloorPlane = plane.id;
        plane.label = mrpt::format("Floor%u", plane.id);
    }
    else if (cosGravityDir > 0.94)
        plane.label = mrpt::format("Ceiling%u", plane.id);
    else if (fabs(cosGravityDir) < 0.34)
        plane.label = mrpt::format("Wall%u", plane.id);
    else
        return false;

    // For bounding planes (e.g. floor) -> Check that the rest of planes in the
    // map are above this one
    for (unsigned i = 0; i < mPbMap.vPlanes.size(); i++)
    {
        if (plane.id == mPbMap.vPlanes[i].id) continue;
        if (plane.v3normal.dot(mPbMap.vPlanes[i].v3center - plane.v3center) <
            -0.1)
        {
            plane.label = "";
            return false;
        }
    }

    plane.bFromStructure = true;

    return true;
}

/**!
 * Check if the input plane has points too close to the image limits. Assume
 * squared cells downsampling
 * Check that the convex hull vertex are 'dist_threshold' away from the image
 * limits.
 */
bool PlaneInferredInfo::isPlaneCutbyImage(
    vector<int>& planeIndices, unsigned& widthSampledImage,
    unsigned& heightSampledImage, unsigned threshold)
{
    unsigned upperLimWidth = widthSampledImage - threshold;
    unsigned upperLimHeight = heightSampledImage - threshold;
    unsigned u, v;
    // cout << "threshold " << threshold << " upperLimWidth " << upperLimWidth
    // << " upperLimHeight " << upperLimHeight << endl;
    for (unsigned i = 0; i < planeIndices.size(); i++)
    {
        u = planeIndices[i] % widthSampledImage;
        v = planeIndices[i] / widthSampledImage;
        //  cout << "idx " << planeIndices[i] << " u " << u << " v " << v <<
        //  endl;
        if (u < threshold || u > upperLimWidth || v < threshold ||
            v > upperLimHeight)
        {  // cout << "\nRETURN TRUE " << u << " " << v << endl;
            return true;
        }
    }
    // cout << "\n\n\nRETURN FALSE\n";
    return false;
}

/**!
 * Check if the surrounding points of the input plane in the input frame are
 * behind the plane
 */
bool PlaneInferredInfo::isSurroundingBackground(
    Plane& plane, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& frame,
    vector<int>& planeIndices, unsigned threshold)
{
    // cout << "isSurroundingBackground\n";
    // Find in the frame the 2D position of the convex hull vertex by brute
    // force // Innefficient
    vector<pair<unsigned, unsigned>> polyImg;
    for (unsigned i = 0; i < plane.polygonContourPtr->size(); i++)
        for (unsigned j = 0; j < planeIndices.size(); j++)
            if (frame->points[planeIndices[j]].x ==
                    plane.polygonContourPtr->points[i].x &&
                frame->points[planeIndices[j]].y ==
                    plane.polygonContourPtr->points[i].y &&
                frame->points[planeIndices[j]].z ==
                    plane.polygonContourPtr->points[i].z)
            {
                polyImg.push_back(pair<unsigned, unsigned>(
                    planeIndices[j] % frame->width,
                    planeIndices[j] / frame->width));
                break;
            }

    // cout << "polyImg size " << polyImg.size() << endl;

    // Calc center
    unsigned left = polyImg[0].first, right = polyImg[0].first,
             up = polyImg[0].second, down = polyImg[0].second;
    for (unsigned i = 1; i < polyImg.size(); i++)
    {
        if (polyImg[i].first < left)
            left = polyImg[i].first;
        else if (polyImg[i].first > right)
            right = polyImg[i].first;
        if (polyImg[i].second < down)
            down = polyImg[i].second;
        else if (polyImg[i].second > up)
            up = polyImg[i].second;
    }
    pair<int, int> center((left + right) / 2, (up + down) / 2);

    // cout << "center " << center.first << " " << center.second << endl;

    vector<Eigen::Vector2i> outerPolygon;
    unsigned Threshold = threshold * frame->width / 640;
    for (unsigned i = 1; i < polyImg.size(); i++)
    {
        Eigen::Vector2f dir;
        double normDir = sqrt(double(
            (polyImg[i].first - center.first) *
                (polyImg[i].first - center.first) +
            (polyImg[i].second - center.second) *
                (polyImg[i].second - center.second)));
        dir[0] = (polyImg[i].first - center.first) / normDir;
        dir[1] = (polyImg[i].second - center.second) / normDir;
        //  cout << "polyImg[i] " << polyImg[i].first << " " <<
        //  polyImg[i].second << endl;
        //  cout << "dir " << dir << endl;
        //  cout << "normDir " << normDir << endl;
        Eigen::Vector2i outVertex;
        outVertex[0] = dir[0] * Threshold + polyImg[i].first;
        outVertex[1] = dir[1] * Threshold + polyImg[i].second;
        if (outVertex[0] < 0)
            outVertex[0] = 0;
        else if (outVertex[0] >= static_cast<int>(frame->width))
            outVertex[0] = frame->width - 1;
        if (outVertex[1] < 0)
            outVertex[1] = 0;
        else if (outVertex[1] >= static_cast<int>(frame->height))
            outVertex[1] = frame->height - 1;
        //  cout << "outVertex " << outVertex << endl;
        outerPolygon.push_back(outVertex);
    }

    // cout << "Fill outer pc\n";
    // Fill outerPolygonPtr point cloud
    for (unsigned i = 1; i < outerPolygon.size(); i++)
    {
        double edgeLenght = std::sqrt(static_cast<double>(
            (outerPolygon[i][0] - outerPolygon[i - 1][0]) *
                (outerPolygon[i][0] - outerPolygon[i - 1][0]) +
            (outerPolygon[i][1] - outerPolygon[i - 1][1]) *
                (outerPolygon[i][1] - outerPolygon[i - 1][1])));
        Eigen::Vector2f direction;
        direction[0] =
            (outerPolygon[i][0] - outerPolygon[i - 1][0]) / edgeLenght;
        direction[1] =
            (outerPolygon[i][1] - outerPolygon[i - 1][1]) / edgeLenght;
        //  cout << "outerPolygon[i] " << outerPolygon[i] << " outerPolygon[i-1]
        //  " << outerPolygon[i-1] << endl;
        //  cout << "direction " << direction << endl;
        //  cout << "edgeLenght " << edgeLenght << endl;
        //    Eigen::Vector2f direction = normalize(outerPolygon[i] -
        //    outerPolygon[i-1]);
        Eigen::Vector2i outContour;
        for (unsigned j = 0; j < edgeLenght; j++)
        {
            outContour[0] = outerPolygon[i - 1][0] + int(direction[0] * j);
            outContour[1] = outerPolygon[i - 1][1] + int(direction[1] * j);
            //  cout << "outContour " << outContour << endl;
            pcl::PointXYZRGBA& outerPt =
                frame->points[outContour[0] + outContour[1] * frame->width];
            if (!pcl_isfinite(outerPt.x)) continue;

            // Fill pointCloud corresponding to the outerPolygon
            plane.outerPolygonPtr->points.push_back(outerPt);
            double dist2Plane = plane.v3normal.dot(
                getVector3fromPointXYZ(outerPt) - plane.v3center);
            if (dist2Plane > 0.1) return false;
        }
    }

    //  cout << "outerPolygonPtr size " << plane.outerPolygonPtr->size() <<
    //  endl;

    return true;
}

/**!
 * Check if the area of input plane is stable and bounded (e.g < 1 m2) along the
 * last keyframes that observe it
 */
void PlaneInferredInfo::isFullExtent(Plane& plane, double newArea)
{
    // cout << "Plane " << plane.id << " newArea " << newArea << " limit " <<
    // 1.1*plane.areaVoxels<< endl;
    if (plane.areaVoxels > 1.0 || newArea > 1.0)
    {
        plane.bFullExtent = false;
        return;
    }

    if (newArea < 1.1 * plane.areaVoxels)
        plane.nFramesAreaIsStable++;
    else
        plane.nFramesAreaIsStable = 0;

    if (plane.nFramesAreaIsStable > 2)
    {
        plane.bFullExtent = true;
    }
}

#endif