LidarInterpreter.hpp

#ifndef LIDAR_INTERPRETER_HPP
#define LIDAR_INTERPRETER_HPP

#include <cmath>
#include <cstdint>
#include "ISensorInterpreter.hpp"
#include "Vision/ICupDetector.hpp"
#ifdef TESTING
#include "Vision/mock/MockCupDetector.hpp"
#else
#include "Vision/CupDetector.hpp"
#endif
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

namespace HLR
{
namespace PointCloud
{
class LidarInterpreter : public ISensorInterpreter
{
public:
    std::vector<Object> get_objects(
        const HLR::Vision::CupDetection& detected_cups,
        const pcl::PointCloud<pcl::PointXYZ>& point_cloud) 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>;

    struct CupMeasurements
    {
        double upper_radius;
        double lower_radius;
        double height;
        CupMeasurements() = default;

        CupMeasurements(double upper_radius, double lower_radius, double height)
            : upper_radius(upper_radius)
            , lower_radius(lower_radius)
            , height(height)
        {}
    };

    static Object convert_cup_to_polyhedron(
        const Cup& cup_in_robot_arm_space,
        const pcl::PointCloud<pcl::PointXYZ>& point_cloud);
    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(
        std::pair<double, double> angle_and_distance_cup,
        const pcl::PointCloud<pcl::PointXYZ>& newest_cloud);
    static std::pair<double, double> translate_kinect_to_lidar(
        const std::shared_ptr<cv::Mat>& distance_frame,
        const cv::Point& location_cup);
    bool is_angle_in_angle_range(double input,
                                 double min_angle,
                                 double max_angle);
    std::vector<PointCloudWithId> pointcloud_database;
    unsigned long highest_pointcloud_id = 0;
    std::unique_ptr<HLR::Vision::ICupDetector> cup_detector
#ifdef TESTING
        = std::make_unique<HLR::Vision::MockCupDetector>();
#else
        // = std::make_unique<HLR::Vision::CupDetector>();
        ;
#endif

    static constexpr double distance_kinect_lidar = 0.084; // meters
    static constexpr unsigned number_of_cylinder_faces = 24;
    static const CupMeasurements
        measured_cup_values; // = CupMeasurements(0.079, 0.03, 0.0225);
    static constexpr double lidar_height = 0.032;
    static const CGAL::Simple_cartesian<double>::Vector_3
        lidar_to_robot_arm_translation;
};

} // namespace PointCloud
} // namespace HLR

#endif /*LIDAR_INTERPRETER_HPP */