SocialCostmapBuilder.cpp

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2022
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "SocialCostmapBuilder.h"
 
#include <cmath>
#include <cstddef>
 
#include <Eigen/Geometry>
 
#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/SceneObjectSet.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <armarx/navigation/algorithms/types.h>
#include <armarx/navigation/algorithms/util.h>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/basic_types.h>
#include <range/v3/numeric/accumulate.hpp>
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/transform.hpp>
 
namespace armarx::navigation::components::social_cost_map
{
 
 
    SocialCostmapBuilder::SocialCostmapBuilder(const navigation::algorithms::SceneBounds sceneBounds,
                                               const navigation::algorithms::Costmap::Parameters& parameters) :
        sceneBounds(sceneBounds), parameters(parameters)
    {
    }
 
    navigation::algorithms::Costmap
    SocialCostmapBuilder::create(const navigation::human::Humans& humans, Mode mode)
    {
        const auto humanPoses = getHumanPoses(humans);
        return create(humanPoses, mode);
    }
 
    navigation::algorithms::Costmap
    SocialCostmapBuilder::create(const std::vector<navigation::core::Pose2D>& humanPoses, Mode mode)
    {
        const auto grid = createUniformGrid(sceneBounds, parameters);
 
        ARMARX_VERBOSE << "Creating grid";
        navigation::algorithms::Costmap costmap(grid, parameters, sceneBounds);
 
        ARMARX_VERBOSE << "Filling grid with size (" << costmap.getGrid().rows() << "/"
                       << costmap.getGrid().cols() << ")";
 
        switch (mode)
        {
            case Mode::Mode1:
                fillGridCostsMode1(costmap, humanPoses);
                break;
            case Mode::Empty:
                ARMARX_VERBOSE << "Empty mode selected";
                costmap.getMutableGrid().setOnes();
                break;
        }
 
        ARMARX_VERBOSE << "Filled grid";
 
        initializeMask(costmap);
 
        return costmap;
    }
 
    void
    SocialCostmapBuilder::initializeMask(navigation::algorithms::Costmap& costmap)
    {
        costmap.getMutableMask() = costmap.getGrid().array() > 0.F;
 
        ARMARX_VERBOSE << "Initializing mask: Fraction of valid elements: "
                       << costmap.getMask()->cast<float>().sum() / costmap.getMask()->size();
    }
 
    Eigen::MatrixXf
    SocialCostmapBuilder::createUniformGrid(const navigation::algorithms::SceneBounds& sceneBounds,
                                            const navigation::algorithms::Costmap::Parameters& parameters)
    {
        ARMARX_TRACE;
 
        ARMARX_VERBOSE << "Scene bounds are " << sceneBounds.min << " and " << sceneBounds.max;
 
        //+1 for explicit rounding up
        size_t c_x = (sceneBounds.max.x() - sceneBounds.min.x()) / parameters.cellSize + 1;
        size_t c_y = (sceneBounds.max.y() - sceneBounds.min.y()) / parameters.cellSize + 1;
 
        ARMARX_VERBOSE << "Grid size: " << c_x << ", " << c_y;
 
        ARMARX_VERBOSE << "Resetting grid";
        Eigen::MatrixXf grid(c_x, c_y);
        grid.setZero();
 
        return grid;
    }
 
    auto
    toM(Eigen::Isometry3f t) -> Eigen::Isometry3f
    {
        t.translation() *= 0.001;
        return t;
    }
 
    auto
    toM(Eigen::Vector3f t) -> Eigen::Vector3f
    {
        t *= 0.001;
        return t;
    }
 
    float
    SocialCostmapBuilder::computeCostMode1(const navigation::core::Position& position,
                                            const std::vector<navigation::core::Pose2D>& humanPoses)
    {
        ARMARX_TRACE;
        // for real experiments use something greater than 600'000
        //float std_deviation = 600000.0f;
        float std_deviation = 1'000'000.0f;
        float peak_height = 1.0f;
 
        const auto costs =
            humanPoses | ranges::views::transform(
                         [&](const navigation::core::Pose2D& global_T_human_2d) -> float
                         {
                             float result = peak_height * exp(-0.5 * (global_T_human_2d.translation() - position.head<2>()).squaredNorm() / std_deviation);
 
                             return result;
                         });
 
        //return ranges::accumulate(costs, 0.f);
        if (ranges::empty(costs))
        {
            return 0.f;
        }
        return ranges::max(costs);
    }
 
    void
    SocialCostmapBuilder::fillGridCostsMode1(navigation::algorithms::Costmap& costmap, const std::vector<navigation::core::Pose2D>& humanPoses)
    {
 
        const std::size_t c_x = costmap.getGrid().rows();
        const std::size_t c_y = costmap.getGrid().cols();
 
        //ARMARX_INFO << VAROUT(humans.front().estimateAt(Clock::Now()).translation());
 
        for (unsigned int x = 0; x < c_x; x++)
        {
            for (unsigned int y = 0; y < c_y; y++)
            {
                const navigation::algorithms::Costmap::Index index{x, y};
                const navigation::algorithms::Costmap::Position position = costmap.toPositionGlobal(index);
 
                costmap.getMutableGrid()(x, y) =
                    computeCostMode1(armarx::navigation::conv::to3D(position), humanPoses);
            }
        }
    }
 
    std::vector<navigation::core::Pose2D>
    SocialCostmapBuilder::getHumanPoses(const navigation::human::Humans& humans)
    {
        return humans | ranges::views::transform(
                            [](const navigation::human::Human& human) -> navigation::core::Pose2D
                            {
                                return human.estimateAt(Clock::Now());
                            }) | ranges::to_vector;
    }
 
 
}