<commit_before>#include "OccupancyGridMap.hpp"
#include <boost/format.hpp>
#include <maps/tools/VoxelTraversal.hpp>

using namespace maps::grid;
using namespace maps::tools;

void OccupancyGridMap::mergePointCloud(const OccupancyGridMap::PointCloud& pc, const base::Transform3d& pc2grid)
{
    Eigen::Vector3d sensor_origin = pc.sensor_origin_.block(0,0,3,1).cast<double>();
    Eigen::Vector3d sensor_origin_in_grid = pc2grid * sensor_origin;
    Eigen::Vector3i sensor_origin_idx;
    Eigen::Vector3d origin_cell_center;
    if(!VoxelGridBase::toVoxelGrid(sensor_origin_in_grid, sensor_origin_idx) || !VoxelGridBase::fromVoxelGrid(sensor_origin_idx, origin_cell_center))
    {
        std::cerr << "Sensor origin (" << sensor_origin_in_grid.transpose() << ") is outside of the grid! Can't add corresponding point cloud to grid." << std::endl;
        return;
    }
    for(PointCloud::const_iterator it=pc.begin(); it != pc.end(); ++it)
    {
        try
        {
            Eigen::Vector3d measurement = it->getArray3fMap().cast<double>();
            mergePoint(sensor_origin_in_grid, sensor_origin_idx, origin_cell_center, pc2grid * measurement);
        }
        catch(const std::runtime_error& e)
        {
            // TODO use glog or base log for all out prints of this library
            std::cerr << e.what() << std::endl;
        }
    }
}

void OccupancyGridMap::mergePoint(const Eigen::Vector3d& sensor_origin, const Eigen::Vector3d& measurement)
{
    Eigen::Vector3i sensor_origin_idx;
    Eigen::Vector3d origin_cell_center;
    if(VoxelGridBase::toVoxelGrid(sensor_origin, sensor_origin_idx) && VoxelGridBase::fromVoxelGrid(sensor_origin_idx, origin_cell_center))
    {
        mergePoint(sensor_origin, sensor_origin_idx, origin_cell_center, measurement);
    }
    else
        throw std::runtime_error((boost::format("Sensor origin %1% is outside of the grid! Can't add to grid.") % sensor_origin.transpose()).str());
}

void OccupancyGridMap::mergePoint(const Eigen::Vector3d& sensor_origin, Eigen::Vector3i sensor_origin_idx,
                                  Eigen::Vector3d origin_cell_center, const Eigen::Vector3d& measurement)
{
    Eigen::Vector3i measurement_idx;
    if(VoxelGridBase::toVoxelGrid(measurement, measurement_idx))
    {
        std::vector<VoxelTraversal::RayElement> ray;
        VoxelTraversal::computeRay(VoxelGridBase::getVoxelResolution(), sensor_origin, sensor_origin_idx, origin_cell_center, measurement, measurement_idx, ray);

        VoxelCellType& cell = getVoxelCell(measurement_idx);
        cell.updateLogOdds(config.hit_logodds, config.min_logodds, config.max_logodds);

        for(const VoxelTraversal::RayElement& element : ray)
        {
            DiscreteTree<VoxelCellType>& tree = at(element.idx);
            for(int32_t z_idx = element.z_first; z_idx != element.z_last; z_idx += element.z_step)
            {
                tree.getCellAt(z_idx).updateLogOdds(config.miss_logodds, config.min_logodds, config.max_logodds);
            }
        }
    }
    else
        throw std::runtime_error((boost::format("Point %1% or is outside of the grid! Can't add to grid.") % measurement.transpose()).str());
}

bool OccupancyGridMap::isOccupied(const Eigen::Vector3d& point) const
{<commit_msg>fixed a bug in the iteration of z indices of a single ray<commit_after>#include "OccupancyGridMap.hpp"
#include <boost/format.hpp>
#include <maps/tools/VoxelTraversal.hpp>

using namespace maps::grid;
using namespace maps::tools;

void OccupancyGridMap::mergePointCloud(const OccupancyGridMap::PointCloud& pc, const base::Transform3d& pc2grid)
{
    Eigen::Vector3d sensor_origin = pc.sensor_origin_.block(0,0,3,1).cast<double>();
    Eigen::Vector3d sensor_origin_in_grid = pc2grid * sensor_origin;
    Eigen::Vector3i sensor_origin_idx;
    Eigen::Vector3d origin_cell_center;
    if(!VoxelGridBase::toVoxelGrid(sensor_origin_in_grid, sensor_origin_idx) || !VoxelGridBase::fromVoxelGrid(sensor_origin_idx, origin_cell_center))
    {
        std::cerr << "Sensor origin (" << sensor_origin_in_grid.transpose() << ") is outside of the grid! Can't add corresponding point cloud to grid." << std::endl;
        return;
    }
    for(PointCloud::const_iterator it=pc.begin(); it != pc.end(); ++it)
    {
        try
        {
            Eigen::Vector3d measurement = it->getArray3fMap().cast<double>();
            mergePoint(sensor_origin_in_grid, sensor_origin_idx, origin_cell_center, pc2grid * measurement);
        }
        catch(const std::runtime_error& e)
        {
            // TODO use glog or base log for all out prints of this library
            std::cerr << e.what() << std::endl;
        }
    }
}

void OccupancyGridMap::mergePoint(const Eigen::Vector3d& sensor_origin, const Eigen::Vector3d& measurement)
{
    Eigen::Vector3i sensor_origin_idx;
    Eigen::Vector3d origin_cell_center;
    if(VoxelGridBase::toVoxelGrid(sensor_origin, sensor_origin_idx) && VoxelGridBase::fromVoxelGrid(sensor_origin_idx, origin_cell_center))
    {
        mergePoint(sensor_origin, sensor_origin_idx, origin_cell_center, measurement);
    }
    else
        throw std::runtime_error((boost::format("Sensor origin %1% is outside of the grid! Can't add to grid.") % sensor_origin.transpose()).str());
}

void OccupancyGridMap::mergePoint(const Eigen::Vector3d& sensor_origin, Eigen::Vector3i sensor_origin_idx,
                                  Eigen::Vector3d origin_cell_center, const Eigen::Vector3d& measurement)
{
    Eigen::Vector3i measurement_idx;
    if(VoxelGridBase::toVoxelGrid(measurement, measurement_idx))
    {
        std::vector<VoxelTraversal::RayElement> ray;
        VoxelTraversal::computeRay(VoxelGridBase::getVoxelResolution(), sensor_origin, sensor_origin_idx, origin_cell_center, measurement, measurement_idx, ray);

        VoxelCellType& cell = getVoxelCell(measurement_idx);
        cell.updateLogOdds(config.hit_logodds, config.min_logodds, config.max_logodds);

        for(const VoxelTraversal::RayElement& element : ray)
        {
            DiscreteTree<VoxelCellType>& tree = at(element.idx);
            int32_t z_end = element.z_last + element.z_step;
            for(int32_t z_idx = element.z_first; z_idx != z_end; z_idx += element.z_step)
            {
                tree.getCellAt(z_idx).updateLogOdds(config.miss_logodds, config.min_logodds, config.max_logodds);
            }
        }
    }
    else
        throw std::runtime_error((boost::format("Point %1% or is outside of the grid! Can't add to grid.") % measurement.transpose()).str());
}

bool OccupancyGridMap::isOccupied(const Eigen::Vector3d& point) const
{
{'fraction_non_alphanumeric': 0.11459589867310012, 'fraction_numerical': 0.008745476477683957, 'mean_word_length': 3.972263868065967, 'pattern_counts': {'":': 0, '<': 39, '<?xml version=': 0, '>': 17, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 2, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '218420', 'n_tokens_mistral': 2164, 'n_tokens_neox': 2104, 'n_words': 388}