agv_ros_ws/space_slam/map/map.cpp

#include "map.h"

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

namespace hlzn_slam
{
float normalDistribution(const float x, const float expectation, const float variance) {
    return exp(-(x - expectation) * (x - expectation) / (2 * variance * variance)) / (sqrt(2 * 3.14) * variance);
}

Eigen::MatrixXf __gaussian(int size, float resolution, float u)
{
    const int window_size = 3;
    // const float variance = window_size * resolution / 3.f;
    Eigen::MatrixXf gaussian_window = Eigen::MatrixXf::Zero(2 * window_size + 1, 2 * window_size + 1);
    const float max_expectation = normalDistribution(0, 0, u);

    for (int i = -window_size; i < window_size + 1; i++) {
        for (int j = -window_size; j < window_size + 1; j++) {
            const float x = resolution * sqrt(i * i + j * j);
            
            gaussian_window(i + window_size, j + window_size)
                    = normalDistribution(x, 0, u) / max_expectation;
        }
    }
    return gaussian_window;
}

void gaussProbabilityMap(Eigen::MatrixXf* input_map, Eigen::MatrixXf* out_map, const float resolution)
{
    const int window_size = 3;
    const float variance = window_size * resolution / 3.f;
    Eigen::MatrixXf gaussian_window = Eigen::MatrixXf::Zero(2 * window_size + 1, 2 * window_size + 1);
    const float max_expectation = normalDistribution(0, 0, variance);

    for (int i = -window_size; i < window_size + 1; i++) {
        for (int j = -window_size; j < window_size + 1; j++) {
            const float x = resolution * sqrt(i * i + j * j);
            
            gaussian_window(i + window_size, j + window_size)
                    = normalDistribution(x, 0, variance) / max_expectation;
        }
    }

    for (int y = 0; y < input_map->rows(); y++) {
        for (int x = 0; x < input_map->cols(); x++) {
            if ((*input_map)(y, x) > 0) {
                for (int i = -window_size; i < window_size + 1; i++) {
                    for (int j = -window_size; j < window_size + 1; j++) {
                        bool inmap = (y + i > -1) && (y + i < input_map->rows()) 
                                        && (x + j > -1) && (x + j < input_map->cols());
                        if (inmap) {
                            float p = (*out_map)(y + i, x + j);
                            float q = gaussian_window(i + window_size, j + window_size);
                            
                            // if (i == 0 && j == 0) {
                            //     (*out_map)(y + i, x + j) = (p + q) > 1 ? 1 : (p + q);
                            // }
                            // else {
                                (*out_map)(y + i, x + j) = std::max(p, q);
                            // }
                             //cost_map_ is same as Hmap.lookup original
                            // if (p < q)
                            //     (*out_map)(y + i, x + j) = q;
                        }
                    }
                }
            }
        }
    }
}

inline int RoundToInt(const float x) { return std::lround(x); }
inline int RoundToInt(const double x) { return std::lround(x); }

Map::Map(float length, float width, float resolution) : resolution_(resolution),
                                                        reciprocal_resolution_(1.0 / resolution_),
                                                        rows_(std::ceil(length / resolution)), 
                                                        cols_(std::ceil(width / resolution)),
                                                        width_(width),
                                                        length_(length),
                                                        x_origin_(length * 0.5), 
                                                        y_origin_(width * 0.5),
                                                        use_shadow_(false)
{
    map_.setZero(rows_, cols_);
    shadow_map_.setZero(rows_, cols_);
}

Map::Map(Eigen::MatrixXf& map, float resolution, float x_origin, float y_origin) : use_shadow_(false), x_origin_(x_origin), y_origin_(y_origin)
{
    __sharpenMap(map);

    Eigen::MatrixXf guss_map = map;
    
    gaussProbabilityMap(&map, &guss_map, resolution);
    map_ = guss_map;
    rows_ = map_.rows();
    cols_ = map_.cols();

    shadow_map_.setZero(rows_, cols_);

    // for (int i = 0;i < rows_;i++) {
    //     for (int j = 0;j < cols_;j++) {
    //         if (map_(i, j) != 0) {
    //             std::tuple<int /*row*/, int /*col*/> index = 
    //                 std::make_tuple(i, j);

    //             index_table_.emplace(index, &map_(i, j));
    //         }
    //     }
    // }

    resolution_ = resolution;
    reciprocal_resolution_ = 1.0 / resolution_;
    width_ = static_cast<float>(rows_) * resolution;
    length_ = static_cast<float>(cols_) * resolution;
}

void Map::pixelValuePlus(const std::vector<Eigen::Vector3f>& pinot_cloud, const float& probablity, const float& upper)
{
    for (const Eigen::Vector3f& a : pinot_cloud) {
        int row, col;

        __world2Map(a(0), a(1), row, col);
        if (__outBound(row, col)) {
            continue;
        }
        __increaseProbablity(row, col, probablity, upper);
    }
    // __normProbablity(upper);
}

void Map::pixelValuePlus2(const std::vector<Eigen::Vector3f>& pinot_cloud, const float& probablity, const float& upper)
{
    for (const Eigen::Vector3f& a : pinot_cloud) {
        int row, col;

        __world2Map(a(0), a(1), row, col);
        if (__outBound(row, col)) {
            continue;
        }
        if (__read(row, col) < 1e-3) {
            __increaseProbablity2(row, col, probablity, upper);
        }
    }
}

void Map::pixelValuePlus3(const std::vector<Eigen::Vector3f>& pinot_cloud, const float& probablity, const float& upper)
{
    for (const Eigen::Vector3f& a : pinot_cloud) {
        int row, col;

        __world2Map(a(0), a(1), row, col);
        if (__outBound(row, col)) {
            continue;
        }
        std::tuple<int /*row*/, int /*col*/> index = std::make_tuple(row, col);
        std::map<const std::tuple<int /*row*/, int /*col*/>, 
                float* /*概率*/>::iterator table_iterator = index_table_.find(index);

        if (__read(row, col) < 1e-3 || table_iterator != index_table_.end()) {
            __increaseProbablity2(row, col, probablity, upper);
        }
    }
}

void Map::__normProbablity(const float& upper)
{
    float max = 0.0;

    for(auto& c : index_table_) {
        max = std::max(max, *c.second);
    }
    if (max > upper) {
        float norm = (upper / max);

        for(auto& c : index_table_) {
            *c.second *= norm;
        }
    }
}

void Map::pixelValuePlusShadow(const std::vector<Eigen::Vector3f>& pinot_cloud, const float& probablity, const float& upper)
{
    for (const Eigen::Vector3f& a : pinot_cloud) {
        int row, col;

        __world2Map(a(0), a(1), row, col);
        if (__outBound(shadow_map_, row, col)) {
            continue;
        }
        __increaseProbablityShadow(row, col, probablity, upper);
    }
    float max = 0.0;

    for(auto& c : shadow_) {
        max = std::max(max, *c.second);
    }
    if (max > upper) {
        float norm = (upper / max);

        for(auto& c : shadow_) {
            *c.second *= norm;
        }
    }
}

void Map::pixelValuePlusShadow2(const std::vector<Eigen::Vector3f>& pinot_cloud, const float& probablity, const float& upper)
{
    for (const Eigen::Vector3f& a : pinot_cloud) {
        int row, col;

        __world2Map(a(0), a(1), row, col);
        if (!__outBound(shadow_map_, row, col)) {
            if (__readShadow(row, col) < 1e-3) {
                __increaseProbablityShadow2(row, col, probablity, upper);
            }
        }
    }
}

void Map::mapAttenuate(const float& attenuate_value)
{
    std::map<const std::tuple<int /*row*/, int /*col*/>, 
            float* /*概率*/>::iterator iter = index_table_.begin();
    
    for (;iter != index_table_.end();iter++) {
        *(iter->second) -= attenuate_value;

        if (*(iter->second) < 0) {
            index_table_.erase(iter);
        }
    }
}

void Map::mapShadowAttenuate(const float& attenuate_value)
{
    std::map<const std::tuple<int /*row*/, int /*col*/>, 
            float* /*概率*/>::iterator iter = shadow_.begin();
    
    for (;iter != shadow_.end();iter++) {
        *iter->second -= attenuate_value;

        if (*iter->second < 0) {
            shadow_.erase(iter);
        }
    }
}

void Map::out(std::string path)
{
    cv::Mat mat(cv::Size(cols_, rows_), CV_8U, cv::Scalar(0));

    for(auto& c : index_table_) {
        if (__outBound(std::get<0>(c.first), std::get<1>(c.first))) {
            continue;
        }
        unsigned char *p;
        unsigned char pix = 0;        
        char value = (char)(__read(std::get<0>(c.first), std::get<1>(c.first)) * 255);
        
        p = mat.ptr<uchar>(std::get<0>(c.first));
        *(p + std::get<1>(c.first)) = value;
    }
    for(auto& c : shadow_) {
        if (__outBound(std::get<0>(c.first), std::get<1>(c.first))) {
            continue;
        }
        unsigned char *p;
        unsigned char pix = 0;        
        char value = (char)(__readShadow(std::get<0>(c.first), std::get<1>(c.first)) * 255);
        
        p = mat.ptr<uchar>(std::get<0>(c.first));
        *(p + std::get<1>(c.first)) = value;
    }
    cv::imwrite(path, mat);
}

float Map::getCost(const common::PointCloud& point_cloud)
{
    if (point_cloud.data.empty()) {
        return 0.0;
    }
    
    float cost = 0;

    for (const Eigen::Vector3f& point : point_cloud.data) {
        int row, col;

        __world2Map(point(0), point(1), row, col);

        cost += getPixelProbablity(row, col);
    }
    cost /= static_cast<float>(point_cloud.data.size());

    return cost;
}

float Map::getPixelProbablity(const int row, const int col)
{
    float probablity = 0;
    
    probablity += __read(row, col);
    if (use_shadow_) {
        probablity += __readShadow(row, col);
    }

    probablity = probablity > 1.0 ? 1.0 : probablity;

    return probablity;
}

void Map::useShadow(bool ok)
{
    use_shadow_ = ok;
}

int Map::getRows()
{
    return rows_;
}

int Map::getCols()
{
    return cols_;
}

float Map::getLength()
{
    return length_;
}

float Map::getWidth()
{
    return width_;
}

float Map::getResolution()
{
    return resolution_;
}

const float& Map::getXorigin()
{
    return x_origin_;
}

const float& Map::getYorigin()
{
    return y_origin_;
}

const float& Map::getReciprocalResolution()
{
    return reciprocal_resolution_;
}

Eigen::MatrixXf& Map::getMapCite()
{
    return map_;
}

Eigen::MatrixXf* Map::getMapPtr()
{
    return &map_;
}

void Map::clearMap()
{
    __clearMap();
}

void Map::clearShadowMap()
{
    shadow_.clear();
}

bool Map::empty()
{
    return index_table_.empty();
}

void Map::word2Map(float x, float y, int& row, int& col)
{
    __world2Map(x, y, row, col);
}

void Map::weakenPixel(float x, float y, float value)
{
    int row, col;

    __world2Map(x, y, row, col);

    std::tuple<int /*row*/, int /*col*/> index = std::make_tuple(row, col);
    std::map<const std::tuple<int /*row*/, int /*col*/>, 
            float* /*概率*/>::iterator table_iterator = index_table_.find(index);

    if (table_iterator != index_table_.end()) {
        *(table_iterator->second) -= value;

        if (*(table_iterator->second) < 0) {
            *(table_iterator->second) = 0.0;
            index_table_.erase(table_iterator);
        }
    }
}

void Map::__world2Map(float x, float y, int& row, int& col)
{
    // ros的坐标系在右手坐标系上旋转了90度，使得x轴指向图片的列
    row = RoundToInt(getRows()  - (y + y_origin_) * reciprocal_resolution_);
    col = RoundToInt((x + x_origin_) * reciprocal_resolution_);
}

float& Map::__read(int row, int col)
{
    static float zero = 0.0;

    if (__outBound(row, col)) {
        return zero;
    }
    return map_(row, col);
}

float& Map::__read(Eigen::MatrixXf& map, int row, int col)
{
    static float zero = 0.0;

    if (__outBound(map, row, col)) {
        return zero;
    }
    return map(row, col);
}

void Map::__writeMap(int row, int col, float value)
{
    if (__outBound(row, col)) {
        return;
    }
    map_(row, col) = value;
}

float Map::__readShadow(int row, int col)
{
    float shadow_value = 0;

    std::tuple<int /*row*/, int /*col*/> index = 
        std::make_tuple(row, col);
    auto table_iterator = shadow_.find(index);

    if (table_iterator != shadow_.end()) {
        shadow_value = *table_iterator->second;
    }
    return shadow_value;
} 

void Map::__increaseProbablity(int& row, int& col, const float& probablity, const float& upper)
{
    if (__outBound(row, col)) {
        return;
    }

    std::tuple<int /*row*/, int /*col*/> index = std::make_tuple(row, col);
    std::map<const std::tuple<int /*row*/, int /*col*/>, 
            float* /*概率*/>::iterator table_iterator = index_table_.find(index);
    float plus = probablity;

    if (table_iterator == index_table_.end()) {
        auto iter = index_table_.emplace(index, &map_(row, col));

        *(iter.first->second) += plus;
    }
    else {

        *(table_iterator->second) += plus;
    }
    return;
}

void Map::__increaseProbablity2(int& row, int& col, const float& probablity, const float& upper)
{
    if (__outBound(row, col)) {
        return;
    }

    std::tuple<int /*row*/, int /*col*/> index = std::make_tuple(row, col);
    std::map<const std::tuple<int /*row*/, int /*col*/>, 
            float* /*概率*/>::iterator table_iterator = index_table_.find(index);
    float plus = probablity;

    if (table_iterator == index_table_.end()) {
        auto iter = index_table_.emplace(index, &map_(row, col));

        if (*(iter.first->second) > (upper - probablity)) {
            plus = upper - *(iter.first->second);
        }
        plus < 0 ? plus = 0.0 : true;

        *(iter.first->second) += plus;
    }
    else {
        if (*(table_iterator->second) > (upper - probablity)) {
            plus = upper - *(table_iterator->second);
        }
        plus < 0 ? plus = 0.0 : true;

        *(table_iterator->second) += plus;
    }
    return;
}


void Map::__increaseProbablityShadow(int& row, int& col, const float& probablity, const float& upper)
{
    std::tuple<int /*row*/, int /*col*/> index = 
        std::make_tuple(row, col);
    auto table_iterator = shadow_.find(index);
    float plus = probablity;

    if (table_iterator == shadow_.end()) {
        auto iter = shadow_.emplace(index, &shadow_map_(row, col));
    }
    else {
        *table_iterator->second += plus;
    }
    return;
}

void Map::__increaseProbablityShadow2(int& row, int& col, const float& probablity, const float& upper)
{
    std::tuple<int /*row*/, int /*col*/> index = 
        std::make_tuple(row, col);
    auto table_iterator = shadow_.find(index);
    float plus = probablity;

    if (table_iterator == shadow_.end()) {
        auto iter = shadow_.emplace(index, &shadow_map_(row, col));
    }
    else {
        if (*table_iterator->second > (upper - probablity)) {
            plus = upper - *table_iterator->second;
        }
        plus < 0 ? plus = 0.0 : true;

        *table_iterator->second += plus;
    }
    return;
}

void Map::__clearMap()
{
    index_table_.clear();
}

bool Map::__outBound(int row, int col)
{
    if (row < 0 || col < 0) {
        return true;
    }
    if (map_.rows() > row && map_.cols() > col) {
        return false;
    }
    return true;
}

bool Map::__outBound(Eigen::MatrixXf& map, int row, int col)
{
    if (row < 0 || col < 0) {
        return true;
    }
    if (map.rows() > row && map.cols() > col) {
        return false;
    }
    return true;
}

void Map::__sharpenMap(Eigen::MatrixXf& map)
{
    for (int row = 0; row < map.rows(); row += 3) {
        for (int col = 0; col < map.cols(); col += 3) {
            float max = 
                std::max(
                    std::max(
                        std::max(
                            std::max(
                                std::max(
                                    std::max(
                                        std::max(
                                            std::max(
                                                __read(map, row, col), 
                                                __read(map, row, col + 1)), 
                                                __read(map, row, col + 2)), 
                                                __read(map, row + 1, col)), 
                                                __read(map, row + 1, col + 1)),
                                                __read(map, row + 1, col + 2)), 
                                                __read(map, row + 2, col)),
                                                __read(map, row + 2, col + 1)),
                                                __read(map, row + 2, col + 2));
        
            if (max > 1e-3) {
                float k = 1.0 / max;

                if (!__outBound(map, row, col)) {
                    map(row, col) *= k;
                }
                if (!__outBound(map, row, col + 1)) {
                    map(row, col + 1) *= k;
                }
                if (!__outBound(map, row, col + 2)) {
                    map(row, col + 2) *= k;
                }
                if (!__outBound(map, row + 1, col)) {
                    map(row + 1, col) *= k;
                }
                if (!__outBound(map, row + 1, col + 1)) {
                    map(row + 1, col + 1) *= k;
                }
                if (!__outBound(map, row + 1, col + 2)) {
                    map(row + 1, col + 2) *= k;
                }
                if (!__outBound(map, row + 2, col)) {
                    map(row + 2, col) *= k;
                }
                if (!__outBound(map, row + 2, col + 1)) {
                    map(row + 2, col + 1) *= k;
                }
                if (!__outBound(map, row + 2, col + 2)) {
                    map(row + 2, col + 2) *= k;
                }
            }
        }
    }
}

} // namespace name