LidarInterpreter.hpp

#ifndef LIDAR_INTERPRETER_HPP
#define LIDAR_INTERPRETER_HPP

#include <cstdint>
#include "ISensorInterpreter.hpp"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

namespace HLR
{
namespace PointCloud
{
class LidarInterpreter : public ISensorInterpreter
{
public:
    LidarInterpreter() = default;
    std::vector<Object> get_objects(
        const HLR::Vision::CupDetection& detected_cups) override;

private:
    struct Cup
    {
        Object::Point centerpoint;
        double upper_radius;
        double lower_radius;
        double height;
        std::uint32_t id;
    };
    using PointCloudWithId =
        std::pair<pcl::PointCloud<pcl::PointXYZ>, std::uint32_t>;

    static Object convert_cup_to_polyhedron(const Cup& cup_in_robot_arm_space);
    static Cup translate_lidar_to_robot_arm(const Cup& cup_in_lidar_space);
    static Cup get_centerpoint_from_contour(
        const PointCloudWithId& point_cloud_with_id);
    PointCloudWithId find_contour(double angle);
    static double translate_kinect_to_lidar(
        const std::shared_ptr<cv::Mat>& frame,
        const cv::Point& point);
};

} // namespace PointCloud
} // namespace HLR

#endif /*LIDAR_INTERPRETER_HPP */