grid_conversion.cpp

Go to the documentation of this file.

#include "grid_conversion.h"
 
#include <algorithm>
#include <cstdint>
 
#include <Eigen/Geometry>
 
#include <opencv2/core.hpp>
#include <opencv2/core/mat.hpp>
#include <opencv2/opencv.hpp>
 
#include "RobotAPI/libraries/armem_vision/types.h"
#include "RobotAPI/libraries/core/FramedPose.h"
 
#include <cartographer/mapping/2d/grid_2d.h>
#include <cartographer/mapping/2d/probability_grid.h>
#include <cartographer/mapping/2d/submap_2d.h>
#include <cartographer/mapping/2d/xy_index.h>
#include <cartographer/mapping/internal/2d/tsdf_2d.h>
 
namespace armarx::localization_and_mapping::cartographer_adapter
{
    ColoredMeshGrid::ColoredMeshGrid(Eigen::Array2i numCells) :
        num_cells(numCells),
        vertices(numCells.x(), std::vector<Position>(numCells.y(), Position::Zero())),
        colors(numCells.x(), std::vector<Color>(numCells.y(), Color::Zero()))
    {
    }
 
    void
    ColoredMeshGrid::setColoredVertex(const Eigen::Array2i& idx,
                                      const Position& position,
                                      const Color& color)
    {
        vertices.at(idx.x()).at(idx.y()) = position;
        colors.at(idx.x()).at(idx.y()) = color;
    }
 
    ColoredMeshGrid
    toMeshGrid(const ::cartographer::mapping::ProbabilityGrid& grid)
    {
        const auto resolution = static_cast<float>(grid.limits().resolution());
 
        Eigen::Array2i offset;
        ::cartographer::mapping::CellLimits mapLimits;
        grid.ComputeCroppedLimits(&offset, &mapLimits);
 
        ::cartographer::mapping::XYIndexRangeIterator iter(mapLimits);
 
        ColoredMeshGrid meshGrid({mapLimits.num_x_cells, mapLimits.num_y_cells});
 
        constexpr uint8_t transparency{255}; // non transparent
        const ColoredMeshGrid::Color colorCellUnknown =
            ColoredMeshGrid::Color::Zero(); // fully transparent
 
        std::for_each(iter.begin(),
                      iter.end(),
                      [&](const Eigen::Array2i& cellIndexCropped)
                      {
                          const auto cellIndex = cellIndexCropped + offset;
 
                          const float probability = grid.GetProbability(cellIndex);
                          const bool isKnown = grid.IsKnown(cellIndex);
 
                          const ColoredMeshGrid::Color color = [&]() -> ColoredMeshGrid::Color
                          {
                              if (isKnown)
                              {
                                  const auto c = static_cast<uint8_t>((1 - probability) * 255);
                                  return {c, c, c, transparency};
                              }
 
                              return colorCellUnknown;
                          }();
 
                          ColoredMeshGrid::Position position = ColoredMeshGrid::Position::Zero();
 
                          // x and y are swapped on purpose.
                          position.y() = -cellIndex.x() * resolution;
                          position.x() = -cellIndex.y() * resolution;
 
                          meshGrid.setColoredVertex(cellIndexCropped, position, color);
                      });
 
        // x and y are swapped on purpose.
        // see cartographer/mapping/2d/probability_grid.cc
        const double maxX = grid.limits().max().x();
        const double maxY = grid.limits().max().y();
 
        meshGrid.innerSlicePosition = Eigen::Vector3f(maxX, maxY, 0.);
 
        return meshGrid;
    }
 
    armem::vision::OccupancyGrid
    toOccupancyGrid(const ::cartographer::mapping::ProbabilityGrid& grid)
    {
        const auto resolution = static_cast<float>(grid.limits().resolution());
 
        Eigen::Array2i offset = Eigen::Array2i::Zero();
        ::cartographer::mapping::CellLimits mapLimits = grid.limits().cell_limits();
        grid.ComputeCroppedLimits(&offset, &mapLimits);
 
        armem::vision::OccupancyGrid::Grid occGrid(mapLimits.num_y_cells, mapLimits.num_x_cells);
 
        ::cartographer::mapping::XYIndexRangeIterator iter(mapLimits);
 
        std::for_each(iter.begin(),
                      iter.end(),
                      [&](const Eigen::Array2i& cellIndexCropped)
                      {
                          const auto cellIndex = cellIndexCropped + offset;
 
                          const float probability = grid.GetProbability(cellIndex);
                          const bool isKnown = grid.IsKnown(cellIndex);
 
                          // x and y are mirrored
                          occGrid(mapLimits.num_y_cells - 1 - cellIndexCropped.y(),
                                  mapLimits.num_x_cells - 1 - cellIndexCropped.x()) =
                              isKnown ? probability : 0.F;
                      });
 
        // x and y are swapped on purpose.
        // see cartographer/mapping/2d/probability_grid.cc
 
        const int inner_shift_x =
            grid.limits().cell_limits().num_x_cells - (offset.x() + mapLimits.num_x_cells);
        const int inner_shift_y =
            grid.limits().cell_limits().num_y_cells - (offset.y() + mapLimits.num_y_cells);
 
        const float maxY = resolution - grid.limits().max().y() + resolution * inner_shift_y;
        const float maxX = resolution - grid.limits().max().x() + resolution * inner_shift_x;
 
        return armem::vision::OccupancyGrid{
            .resolution = resolution,
            .frame = MapFrame,
            // Intentionally, we swap x and y. See comment above.
            .pose = Eigen::Isometry3f(Eigen::Translation3f(maxY, maxX, 0.F)),
            .grid = occGrid};
    }
 
    armem::vision::OccupancyGrid
    toOccupancyGrid(const ::cartographer::mapping::Grid2D& grid)
    {
        const auto* probGrid = dynamic_cast<const ::cartographer::mapping::ProbabilityGrid*>(&grid);
        if (probGrid != nullptr)
        {
            auto occ = toOccupancyGrid(*probGrid);
            return occ;
        }
 
        throw InvalidArgumentException();
    }
 
    ColoredMeshGrid
    toMeshGrid(const ::cartographer::mapping::Grid2D& grid)
    {
        const auto* probGrid = dynamic_cast<const ::cartographer::mapping::ProbabilityGrid*>(&grid);
 
        if (probGrid != nullptr)
        {
            return toMeshGrid(*probGrid);
        }
 
        // const auto* tsdf = dynamic_cast<const cartographer::mapping::TSDF2D*>(&grid);
        // if (tsdf)
        // {
        //     ARMARX_INFO << "Processing tsdf grid";
        //     return toMeshGrid(*tsdf, resolution);
        // }
 
        throw InvalidArgumentException();
    }
 
    cv::Mat2b
    toImage(const ::cartographer::mapping::ProbabilityGrid& grid)
    {
        Eigen::Array2i offset;
        ::cartographer::mapping::CellLimits mapLimits;
        grid.ComputeCroppedLimits(&offset, &mapLimits);
 
        ::cartographer::mapping::XYIndexRangeIterator iter(mapLimits);
 
        cv::Mat1f outputMap(mapLimits.num_x_cells, mapLimits.num_y_cells);
        cv::Mat1b outputMapMask(mapLimits.num_x_cells, mapLimits.num_y_cells);
 
        std::for_each(iter.begin(),
                      iter.end(),
                      [&](const ::Eigen::Array2i& cellIndexCropped)
                      {
                          const auto cellIndex = cellIndexCropped + offset;
                          const cv::Point idx{cellIndexCropped[1], cellIndexCropped[0]};
 
                          outputMap.at<float>(idx) = grid.GetProbability(cellIndex);
                          outputMapMask.at<uint8_t>(idx) =
                              static_cast<uint8_t>(grid.IsKnown(cellIndex));
                      });
 
        outputMap *= std::numeric_limits<uint8_t>::max(); // Scale [0..1] -> [0..255]
        outputMapMask *= std::numeric_limits<uint8_t>::max(); // Scale [0..1] -> [0..255]
 
        cv::Mat1b out;
        outputMap.convertTo(out, CV_8UC1);
 
        cv::Mat1b outMask;
        outputMapMask.convertTo(outMask, CV_8UC1);
        outputMapMask *= std::numeric_limits<uint8_t>::max(); // Scale {0,1} -> {0,255}
 
        cv::Mat2b outWithMask;
        cv::merge(std::vector({out, outMask}), outWithMask);
 
        return outWithMask;
    }
 
    cv::Mat2b
    toImage(const ::cartographer::mapping::TSDF2D& tsdf)
    {
        const ::cartographer::mapping::CellLimits& mapLimits = tsdf.limits().cell_limits();
 
        ::cartographer::mapping::XYIndexRangeIterator iter(mapLimits);
 
        cv::Mat1f outputMap(mapLimits.num_x_cells, mapLimits.num_y_cells);
        cv::Mat1f outputMapMask(mapLimits.num_x_cells, mapLimits.num_y_cells);
 
        std::for_each(iter.begin(),
                      iter.end(),
                      [&](const ::Eigen::Array2i& cellIndex)
                      {
                          const cv::Point idx{cellIndex[1], cellIndex[0]};
 
                          outputMap.at<float>(idx) = tsdf.GetTSD(cellIndex);
                          outputMapMask.at<bool>(idx) = tsdf.IsKnown(cellIndex);
                      });
 
        outputMap *= std::numeric_limits<uint8_t>::max(); // Scale [0..1] -> [0..255]
 
        cv::Mat1b out;
        outputMap.convertTo(out, CV_8UC1);
 
        cv::Mat1b outMask;
        outputMapMask.convertTo(outMask, CV_8UC1);
        outputMapMask *= std::numeric_limits<uint8_t>::max(); // Scale {0,1} -> {0,255}
 
        cv::Mat2b outWithMask;
        cv::merge(std::vector({out, outMask}), outWithMask);
 
        return outWithMask;
    }
 
    cv::Mat2b
    toImage(const ::cartographer::mapping::Grid2D& grid)
    {
        const auto* probGrid = dynamic_cast<const ::cartographer::mapping::ProbabilityGrid*>(&grid);
 
        if (probGrid != nullptr)
        {
            return toImage(*probGrid);
        }
 
        const auto* tsdf = dynamic_cast<const ::cartographer::mapping::TSDF2D*>(&grid);
        if (tsdf != nullptr)
        {
            return toImage(*tsdf);
        }
 
        return {};
    }
 
} // namespace armarx::localization_and_mapping::cartographer_adapter