ManagedObstacle.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/>.
 *
 * @package    armar6_skills::ArmarXObjects::DynamicObstacleManager
 * @author     Fabian PK ( fabian dot peller-konrad at kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ManagedObstacle.h"
 
// STD/STL
#include <string>
#include <vector>
 
// Ice
#include <Ice/Current.h>
using namespace Ice;
 
// OpenCV
#include <opencv2/core.hpp>
 
// Eigen
#include <Eigen/Core>
 
// ArmarX
#include <opencv2/core/core_c.h>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/interface/core/PoseBase.h>
#include <RobotAPI/libraries/core/Pose.h>
 
using namespace Eigen;
using namespace armarx;
 
namespace armarx
{
 
    double
    ManagedObstacle::calculateObstacleArea(const obstacledetection::Obstacle& o)
    {
        return M_PI * std::abs(o.axisLengthX) * std::abs(o.axisLengthY);
    }
 
    bool
    ManagedObstacle::point_ellipsis_coverage(const Eigen::Vector2f e_origin,
                                             const float e_rx,
                                             const float e_ry,
                                             const float e_yaw,
                                             const Eigen::Vector2f point)
    {
        // See https://stackoverflow.com/questions/7946187/point-and-ellipse-rotated-position-test-algorithm
        const float sin = std::sin(e_yaw);
        const float cos = std::cos(e_yaw);
 
        const float a = cos * (point(0) - e_origin(0)) + sin * (point(1) - e_origin(1));
        const float b = sin * (point(0) - e_origin(0)) - cos * (point(1) - e_origin(1));
 
        const float e_rx_sq = e_rx * e_rx;
        const float e_ry_sq = e_ry * e_ry;
 
        return a * a * e_ry_sq + b * b * e_rx_sq <= e_rx_sq * e_ry_sq;
    }
 
    float
    ManagedObstacle::ellipsis_ellipsis_coverage(const Eigen::Vector2f e1_origin,
                                                const float e1_rx,
                                                const float e1_ry,
                                                const float e1_yaw,
                                                const Eigen::Vector2f e2_origin,
                                                const float e2_rx,
                                                const float e2_ry,
                                                const float e2_yaw)
    {
        // We use a very simple approach here: We sample points in one ellipsis and check whether it lies in the other (https://www.quora.com/Is-it-possible-to-generate-random-points-within-a-given-rotated-ellipse-without-using-rejection-sampling)
        // For a real approach to intersect two ellipsis see https://arxiv.org/pdf/1106.3787.pdf
 
        unsigned int SAMPLES = 100;
        unsigned int matches = 0;
        for (unsigned int i = 0; i < SAMPLES; ++i)
        {
            float theta = static_cast<float>(rand()) / static_cast<float>(RAND_MAX / 2 * M_PI);
            float k = sqrt(static_cast<float>(rand()) / static_cast<float>(RAND_MAX));
            Eigen::Vector2f sample(k * e2_rx * cos(theta), k * e2_ry * sin(theta));
            //sample *= Eigen::Rotation2D(e2_yaw);
            sample += e1_origin;
 
            if (ManagedObstacle::point_ellipsis_coverage(e1_origin, e1_rx, e1_ry, e1_yaw, sample))
            {
                matches++;
            }
        }
 
        return (1.0f * matches) / SAMPLES;
    }
 
    float
    ManagedObstacle::line_segment_ellipsis_coverage(const Eigen::Vector2f e_origin,
                                                    const float e_rx,
                                                    const float e_ry,
                                                    const float e_yaw,
                                                    const Eigen::Vector2f line_seg_start,
                                                    const Eigen::Vector2f line_seg_end)
    {
        // Again we discretize the line into n points and we check the coverage of those points
        const Eigen::Vector2f difference_vec = line_seg_end - line_seg_start;
        const Eigen::Vector2f difference_vec_normed = difference_vec.normalized();
        const float distance = difference_vec.norm();
        const unsigned int SAMPLES = std::max(distance / 10.0, 40.0);
 
        const Vector2f difference_start_origin = e_origin - line_seg_start;
        const Vector2f difference_end_origin = e_origin - line_seg_end;
 
        if (difference_start_origin.norm() > std::max(e_rx, e_ry) &&
            distance < std::max(e_rx, e_ry))
        {
            return 0.0;
        }
 
        if (difference_end_origin.norm() > std::max(e_rx, e_ry) && distance < std::max(e_rx, e_ry))
        {
            return 0.0;
        }
 
 
        const float step_size = distance / SAMPLES;
        const Eigen::Vector2f dir = difference_vec_normed * step_size;
 
        unsigned int samples_in_ellipsis = 0;
 
        // Sample over line segment with a fixed size
        for (unsigned int i = 0; i < SAMPLES * 0.5; ++i)
        {
            Eigen::Vector2f start_sample = line_seg_start + i * dir;
            Eigen::Vector2f end_sample = line_seg_end - i * dir;
            unsigned int samples_in_ellipsis_this_iteration = 0;
 
            for (const auto& point : {start_sample, end_sample})
            {
                if (ManagedObstacle::point_ellipsis_coverage(e_origin, e_rx, e_ry, e_yaw, point))
                {
                    ++samples_in_ellipsis;
                    ++samples_in_ellipsis_this_iteration;
                }
            }
 
            if (samples_in_ellipsis_this_iteration == 2)
            {
                // Last two points (from the outside) were inside this ellipsis, so the
                // remaining ones are as well simce ellipsis are concave.  Consider this line
                // segment as explained by the obstacle and bail early.
                const unsigned int remaining_samples = SAMPLES - 2 * (i + 1);
                return (1.0f * samples_in_ellipsis + remaining_samples) / SAMPLES;
            }
        }
        return (1.0f * samples_in_ellipsis) / SAMPLES; // only if one point or no point in ellipsis
    }
 
    void
    ManagedObstacle::update_position(const unsigned int thickness)
    {
        if (line_matches.size() < 2)
        {
            return;
        }
 
        ARMARX_DEBUG << "Obstacle " << m_obstacle.name << " has " << line_matches.size()
                     << " matching line segments since last update";
 
        std::vector<float> x_pos;
        std::vector<float> y_pos;
        for (Eigen::Vector2f& match : line_matches)
        {
            ARMARX_DEBUG << "Match: " << match;
            x_pos.push_back(match(0));
            y_pos.push_back(match(1));
        }
 
        cv::Mat x(x_pos.size(), 1, CV_32F, x_pos.data());
        cv::Mat y(y_pos.size(), 1, CV_32F, y_pos.data());
 
        ARMARX_DEBUG << "X: " << x;
        ARMARX_DEBUG << "Y: " << y;
 
        cv::Mat data(x_pos.size(), 2, CV_32F);
        x.col(0).copyTo(data.col(0));
        y.col(0).copyTo(data.col(1));
 
        ARMARX_DEBUG << "Data: " << data;
 
        cv::PCA pca(data, cv::Mat(), CV_PCA_DATA_AS_ROW, 2);
 
        cv::Mat mean = pca.mean;
        ARMARX_DEBUG << "Mean: " << mean;
        cv::Mat eigenvalues = pca.eigenvalues;
        ARMARX_DEBUG << "Eigenvalues: " << eigenvalues;
        cv::Mat eigenvectors = pca.eigenvectors;
        ARMARX_DEBUG << "Eigenvectors: " << eigenvectors;
 
        Eigen::Vector2f pca_center(mean.at<float>(0, 0), mean.at<float>(0, 1));
        Eigen::Vector2f pca1_direction(eigenvectors.at<float>(0, 0), eigenvectors.at<float>(0, 1));
        Eigen::Vector2f pca2_direction(eigenvectors.at<float>(1, 0), eigenvectors.at<float>(1, 1));
        double pca1_eigenvalue = eigenvalues.at<float>(0, 0);
        double pca2_eigenvalue = eigenvalues.at<float>(1, 0);
 
        ARMARX_DEBUG << "PCA: Mean: " << pca_center;
        ARMARX_DEBUG << "PCA1: Eigenvector: " << pca1_direction
                     << ", Eigenvalue: " << pca1_eigenvalue;
        ARMARX_DEBUG << "PCA2: Eigenvector: " << pca2_direction
                     << ", Eigenvalue: " << pca2_eigenvalue;
 
        // calculate yaw of best line (should be close to current yaw)
        const Eigen::Vector2f difference_vec = pca1_direction;
        const Eigen::Vector2f center_vec = pca_center;
        //const Eigen::Vector2f difference_vec1_normed = difference_vec.normalized();
 
        const float yaw1 = getXAxisAngle(difference_vec);
        const float yaw2 = getXAxisAngle(-1 * difference_vec);
 
        const float diff_origin_yaw1 = std::abs(yaw1 - m_obstacle.yaw);
        const float diff_origin_yaw2 = std::abs(yaw2 - m_obstacle.yaw);
        const float diff_origin_yaw1_opposite = std::abs(2 * M_PI - diff_origin_yaw1);
        const float diff_origin_yaw2_opposite = std::abs(2 * M_PI - diff_origin_yaw2);
 
 
        float yaw = yaw2;
        if ((diff_origin_yaw1 < diff_origin_yaw2 && diff_origin_yaw1 < diff_origin_yaw2_opposite) ||
            (diff_origin_yaw1_opposite < diff_origin_yaw2 &&
             diff_origin_yaw1_opposite < diff_origin_yaw2_opposite))
        {
            yaw = yaw1;
        }
 
        // Print matches to debug drawer
        //const std::string debug_line_name = "line_segment_input_" + std::to_string(m_input_index++);
        //const armarx::Vector3BasePtr start_vec_3d{new Vector3(best_start(0), best_start(1), 50)};
        //const armarx::Vector3BasePtr end_vec_3d{new Vector3(best_end(0), best_end(1), 50)};
        //const armarx::Vector3BasePtr center_vec_3d{new Vector3(center_vec(0), center_vec(1), 50)};
        //const armarx::Vector3BasePtr difference_vec_normed_3d{new Vector3(difference_vec1_normed(0), difference_vec1_normed(1), 0)};
        //debug_drawer->setCylinderVisu(layer_name, debug_line_name, center_vec_3d, difference_vec_normed_3d, max_length, 15.f, armarx::DrawColor{1, 0, 0, 0.8});
        //debug_drawer->setSphereVisu(layer_name, debug_line_name + "_ref", start_vec_3d, armarx::DrawColor{1, 1, 0, 0.8}, 35);
        //debug_drawer->setSphereVisu(layer_name, debug_line_name + "_ref2", end_vec_3d, armarx::DrawColor{0, 1, 0, 0.8}, 35);
 
        // Udate position buffer with new best line
        position_buffer.at(position_buffer_index++) =
            std::make_tuple(center_vec(0),
                            center_vec(1),
                            yaw,
                            sqrt(pca1_eigenvalue) * 1.3,
                            std::max(1.0f * thickness,
                                     static_cast<float>(sqrt(pca2_eigenvalue)) *
                                         1.3f)); // add 30% security margin to length of pca
        position_buffer_index %= position_buffer.size();
 
        position_buffer_fillctr++;
        position_buffer_fillctr =
            std::min(position_buffer_fillctr, static_cast<unsigned int>(position_buffer.size()));
 
        // Calculate means from position buffer
        double sum_x_pos = 0;
        double sum_y_pos = 0;
        double sum_yaw = 0;
        double sum_sin_yaw = 0;
        double sum_cos_yaw = 0;
        double sum_axisX = 0;
        double sum_axisY = 0;
 
        std::string yaws;
        for (unsigned int i = 0; i < position_buffer_fillctr; ++i)
        {
            double current_x_pos = std::get<0>(position_buffer[i]);
            double current_y_pos = std::get<1>(position_buffer[i]);
            double current_yaw = std::get<2>(position_buffer[i]);
            double current_axisX = std::get<3>(position_buffer[i]);
            double current_axisY = std::get<4>(position_buffer[i]);
            sum_x_pos += current_x_pos;
            sum_y_pos += current_y_pos;
            sum_yaw += current_yaw;
            sum_sin_yaw += sin(current_yaw);
            sum_cos_yaw += cos(current_yaw);
            sum_axisX += current_axisX;
            sum_axisY += current_axisY;
 
            yaws += std::to_string(current_yaw) + ", ";
        }
 
        double mean_x_pos = sum_x_pos / position_buffer_fillctr;
        double mean_y_pos = sum_y_pos / position_buffer_fillctr;
        double mean_yaw = atan2(
            (1.0 / position_buffer_fillctr) * sum_sin_yaw,
            (1.0 / position_buffer_fillctr) *
                sum_cos_yaw); //sum_yaw  / position_buffer_fillctr; //std::fmod(sum_yaw, 2.0 * M_PI);
        double mean_axisX = sum_axisX / position_buffer_fillctr;
        double mean_axisY = sum_axisY / position_buffer_fillctr;
        mean_yaw = normalizeYaw(mean_yaw);
 
        // Update position and size of obstacle
        m_obstacle.posX = mean_x_pos;
        m_obstacle.posY = mean_y_pos;
        m_obstacle.refPosX = mean_x_pos;
        m_obstacle.refPosY = mean_y_pos;
        m_obstacle.yaw = mean_yaw;
        //if (mean_axisX > m_obstacle.axisLengthX)
        //{
        m_obstacle.axisLengthX = mean_axisX;
        //}
        //if (mean_axisY > m_obstacle.axisLengthY)
        //{
        m_obstacle.axisLengthY = mean_axisY;
        //}
 
        line_matches.clear();
        m_updated = true;
    }
 
    float
    ManagedObstacle::normalizeYaw(float yaw) const
    {
        float pi2 = 2.0 * M_PI;
        while (yaw < 0)
        {
            yaw += pi2;
        }
        while (yaw > pi2)
        {
            yaw -= pi2;
        }
        return yaw;
    }
 
    float
    ManagedObstacle::getAngleBetweenVectors(const Eigen::Vector2f& v1,
                                            const Eigen::Vector2f& v2) const
    {
        // Returns an angle between 0 and 180 degree (the minimum angle between the two vectors)
        const Eigen::Vector2f v1_vec_normed = v1.normalized();
        const Eigen::Vector2f v2_vec_normed = v2.normalized();
        const float dot_product_vec =
            v1_vec_normed(0) * v2_vec_normed(0) + v1_vec_normed(1) * v2_vec_normed(1);
        float yaw = acos(dot_product_vec);
        return yaw;
    }
 
    float
    ManagedObstacle::getXAxisAngle(const Eigen::Vector2f& v) const
    {
        // Returns an angle between 0 and 360 degree (counterclockwise from x axis)
        const Eigen::Vector2f v_vec_normed = v.normalized();
        const float dot_product_vec = v_vec_normed(0);
        const float cross_product_vec = v_vec_normed(1);
        float yaw = acos(dot_product_vec);
        if (cross_product_vec < 0)
        {
            yaw = 2 * M_PI - yaw;
        }
        return yaw;
    }
} // namespace armarx