<commit_before>//
//  occupancy_grid.cpp
//  robotcontrol2
//
//  Created by <NAME> on 24.09.15.
//  Copyright © 2015 <NAME>. All rights reserved.
//

#include "occupancy_grid.h"
#include "robot_configuration.h" // Data structures and configuration data shared with Arduino controller

#include <assert.h>
#include <cstdlib>
#include <cmath>
#include <algorithm>
#include <unordered_map>

#include <boost/range/size.hpp>
#include <opencv2/imgproc.hpp>

namespace {
    struct SRotatedRect {
        rbt::point<int> m_ptnCenter;
        rbt::size<double> m_szf;
        double m_fAngle;
        
        template<typename Func>
        void for_each_pixel(Func foreach) {
            // Rotate the scaled rectangle.
            // TODO: For numerical precision, it may be better to rotate the rect
            // in world coordinates and then scale.
            // TODO: Use cv::LineIterator instead
            rbt::point<int> apt[] = {
                m_ptnCenter - rbt::size<int>((m_szf/2).rotated(m_fAngle)),
                m_ptnCenter + rbt::size<int>((rbt::size<double>(m_szf.x, -m_szf.y)/2).rotated(m_fAngle)),
                m_ptnCenter + rbt::size<int>((m_szf/2).rotated(m_fAngle)),
                m_ptnCenter + rbt::size<int>((rbt::size<double>(-m_szf.x, m_szf.y)/2).rotated(m_fAngle))
            };
            
            auto rasterize = [](rbt::point<int> ptA, rbt::point<int> ptB, auto foreach) {
                if(ptA.x == ptB.x) {
                    // straight vertical line
                    auto nMin = std::min(ptA.y, ptB.y);
                    auto nMax = std::max(ptA.y, ptB.y);
                    
                    for(int y = nMin; y <= nMax; ++y) {
                        foreach(ptA.x, y);
                    }
                } else {
                    if(ptB.x<ptA.x) {
                        std::swap(ptB, ptA);
                    }
                    
                    auto m = rbt::numeric_cast<double>(ptB.y - ptA.y) / (ptB.x - ptA.x);
                    if(std::abs(m)<=1) { // x-step
                        for(int x = ptA.x; x<= ptB.x; ++x) {
                            foreach(x, rbt::numeric_cast<int>(ptA.y + m * (x - ptA.x)));
                        }
                    } else { // y-step
                        auto nMin = std::min(ptA.y, ptB.y);
                        auto nMax = std::max(ptA.y, ptB.y);
                        for(int y = nMin; y <= nMax; ++y) {
                            foreach(rbt::numeric_cast<int>(ptA.x + (y - ptA.y) / m), y);
                        }
                    }
                }
            };
            
            // TODO: The map could be avoided by sorting the line segments
            std::unordered_map<int, rbt::interval<int>> mapnintvlX;
            for(int i=0; i<boost::size(apt); ++i) {
                rasterize(apt[i], apt[(i+1)%boost::size(apt)], [&](int x, int y) {
                    auto pairitb = mapnintvlX.emplace(y, rbt::interval<int>(x, x));
                    if(!pairitb.second) {
                        pairitb.first->second |= x;
                    }
                });
            }
            boost::for_each(mapnintvlX, [&](auto const& pairnintvlX) {
                for(int x = pairnintvlX.second.begin; x <= pairnintvlX.second.end; ++x) {
                    foreach(rbt::point<int>(x, pairnintvlX.first));
                }
            });
        }
    };
}

COccupancyGrid::COccupancyGrid(rbt::size<int> const& szn, int nScale)
:   m_szn(szn), 
    m_nScale(nScale),
    m_matfMapLogOdds(szn.x, szn.y, CV_32FC1, cv::Scalar(0.0f)),
    m_matnMapObstacle(szn.x, szn.y, CV_8UC1, cv::Scalar(255))
{
    assert(0==szn.x%2 && 0==szn.y%2);
}

COccupancyGrid::COccupancyGrid(COccupancyGrid const& occgrid) 
:   m_szn(occgrid.m_szn), 
    m_nScale(occgrid.m_nScale), 
    m_matfMapLogOdds(occgrid.m_matfMapLogOdds.clone()),
    m_matnMapObstacle(occgrid.m_matnMapObstacle.clone())
{}

void COccupancyGrid::update(rbt::pose<double> const& pose, double fRadAngle, int nDistance) {
    auto UpdateMap = [this](rbt::point<int> const& pt, float fDeltaValue) {
        auto const fOdds = m_matfMapLogOdds.at<float>(pt.y, pt.x) + fDeltaValue;
        m_matfMapLogOdds.at<float>(pt.y, pt.x) = fOdds;
        // Calculating the greyscale map is pretty expensive
        // If we ever need a non-binary version, a lookup table
        // would be useful instead of this:
        // auto const nColor = rbt::numeric_cast<std::uint8_t>(1.0 / ( 1.0 + std::exp( fOdds )) * 255);
        m_matnMapObstacle.at<std::uint8_t>(pt.y, pt.x) = 0 < fOdds ? 0 : 255;
    };

    ForEachCell(
        pose, 
        fRadAngle, nDistance, 
        m_matfMapLogOdds, 
        std::bind(&COccupancyGrid::toGridCoordinates, this, std::placeholders::_1), 
        UpdateMap
    );
    
    // Clear position of robot itself
    SRotatedRect rectRobot{toGridCoordinates(pose.m_pt), rbt::size<double>(c_nRobotWidth, c_nRobotHeight)/m_nScale, pose.m_fYaw};
    rectRobot.for_each_pixel([&](rbt::point<int> const& pt) {
        UpdateMap(pt, -m_matfMapLogOdds.at<float>(pt.y, pt.x) + c_fOccupancyRover); 
    });
}

rbt::point<int> COccupancyGrid::toGridCoordinates(rbt::point<double> const& pt) const {
    return rbt::point<int>(pt/m_nScale) + m_szn/2;
}

rbt::point<int> COccupancyGrid::toWorldCoordinates(rbt::point<int> const& pt) const {
    return (pt - m_szn/2) * m_nScale;
}<commit_msg>Update robot position using cv::fillConvexPoly. Again, that's much faster.<commit_after>//
//  occupancy_grid.cpp
//  robotcontrol2
//
//  Created by <NAME> on 24.09.15.
//  Copyright © 2015 <NAME>. All rights reserved.
//

#include "occupancy_grid.h"
#include "robot_configuration.h" // Data structures and configuration data shared with Arduino controller
#include "error_handling.h"

#include <cstdlib>
#include <cmath>
#include <algorithm>

#include <boost/range/size.hpp>
#include <opencv2/imgproc.hpp>

COccupancyGrid::COccupancyGrid(rbt::size<int> const& szn, int nScale)
:   m_szn(szn), 
    m_nScale(nScale),
    m_matfMapLogOdds(szn.x, szn.y, CV_32FC1, cv::Scalar(0.0f)),
    m_matnMapObstacle(szn.x, szn.y, CV_8UC1, cv::Scalar(255))
{
    ASSERT(0==szn.x%2 && 0==szn.y%2);
}

COccupancyGrid::COccupancyGrid(COccupancyGrid const& occgrid) 
:   m_szn(occgrid.m_szn), 
    m_nScale(occgrid.m_nScale), 
    m_matfMapLogOdds(occgrid.m_matfMapLogOdds.clone()),
    m_matnMapObstacle(occgrid.m_matnMapObstacle.clone())
{}

void COccupancyGrid::update(rbt::pose<double> const& pose, double fRadAngle, int nDistance) {
    ForEachCell(
        pose, 
        fRadAngle, nDistance, 
        m_matfMapLogOdds, 
        std::bind(&COccupancyGrid::toGridCoordinates, this, std::placeholders::_1), 
        [this](rbt::point<int> const& pt, float fDeltaValue) {
            auto const fOdds = m_matfMapLogOdds.at<float>(pt.y, pt.x) + fDeltaValue;
            m_matfMapLogOdds.at<float>(pt.y, pt.x) = fOdds;
            // Calculating the greyscale map is pretty expensive
            // If we ever need a non-binary version, a lookup table
            // would be useful instead of this:
            // auto const nColor = rbt::numeric_cast<std::uint8_t>(1.0 / ( 1.0 + std::exp( fOdds )) * 255);
            m_matnMapObstacle.at<std::uint8_t>(pt.y, pt.x) = 0 < fOdds ? 0 : 255;
        }
    );
    
    // Clear position of robot itself
    rbt::size<double> const szfHalfSize(c_nRobotWidth/2.0, c_nRobotHeight/2.0);
    cv::Point const apt[] = {
        toGridCoordinates(pose.m_pt - szfHalfSize.rotated(pose.m_fYaw)),
        toGridCoordinates(pose.m_pt + rbt::size<double>(szfHalfSize.x, -szfHalfSize.y).rotated(pose.m_fYaw)),
        toGridCoordinates(pose.m_pt + szfHalfSize.rotated(pose.m_fYaw)),
        toGridCoordinates(pose.m_pt + rbt::size<double>(-szfHalfSize.x, szfHalfSize.y).rotated(pose.m_fYaw))
    };
    cv::fillConvexPoly(m_matfMapLogOdds, apt, boost::size(apt), cv::Scalar(c_fOccupancyRover));
    cv::fillConvexPoly(m_matnMapObstacle, apt, boost::size(apt), cv::Scalar(255));
}

rbt::point<int> COccupancyGrid::toGridCoordinates(rbt::point<double> const& pt) const {
    return rbt::point<int>(pt/m_nScale) + m_szn/2;
}

rbt::point<int> COccupancyGrid::toWorldCoordinates(rbt::point<int> const& pt) const {
    return (pt - m_szn/2) * m_nScale;
}
{'fraction_non_alphanumeric': 0.14618823245137127, 'fraction_numerical': 0.012081672103419113, 'mean_word_length': 2.3718940936863544, 'pattern_counts': {'":': 0, '<': 77, '<?xml version=': 0, '>': 69, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '6860082', 'n_tokens_mistral': 3152, 'n_tokens_neox': 3015, 'n_words': 525}