erl_geometry/include/erl_geometry/lidar_frame_3d.hpp at main · ExistentialRobotics/erl_geometry · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
#pragma once

#include "range_sensor_frame_3d.hpp"

#include "erl_common/logging.hpp"
#include "erl_common/yaml.hpp"

#include <absl/container/flat_hash_map.h>

namespace erl::geometry {

    template<typename Dtype>
    class LidarFrame3D : public RangeSensorFrame3D<Dtype> {
        friend class LidarFramePartition3D;

    public:
        using Super = RangeSensorFrame3D<Dtype>;
        using MatrixX = Eigen::MatrixX<Dtype>;
        using Matrix3 = Eigen::Matrix3<Dtype>;
        using Matrix3X = Eigen::Matrix3X<Dtype>;
        using Matrix2X = Eigen::Matrix2X<Dtype>;
        using VectorX = Eigen::VectorX<Dtype>;
        using Vector3 = Eigen::Vector3<Dtype>;
        using Vector2 = Eigen::Vector2<Dtype>;

        struct Setting : public common::Yamlable<Setting, typename Super::Setting> {
            Dtype azimuth_min = -M_PI;
            Dtype azimuth_max = M_PI;
            Dtype elevation_min = -M_PI * 0.5f;
            Dtype elevation_max = M_PI * 0.5f;
            long num_azimuth_lines = 360;
            long num_elevation_lines = 180;

            ERL_REFLECT_SCHEMA(
                Setting,
                ERL_REFLECT_MEMBER(Setting, azimuth_min),
                ERL_REFLECT_MEMBER(Setting, azimuth_max),
                ERL_REFLECT_MEMBER(Setting, elevation_min),
                ERL_REFLECT_MEMBER(Setting, elevation_max),
                ERL_REFLECT_MEMBER(Setting, num_azimuth_lines),
                ERL_REFLECT_MEMBER(Setting, num_elevation_lines));

            std::pair<long, long>
            Resize(Dtype factor);

            std::pair<long, long>
            Resize(Dtype factor_azimuth, Dtype factor_elevation);
        };

    protected:
        std::shared_ptr<Setting> m_setting_ = nullptr;

    public:
        explicit LidarFrame3D(std::shared_ptr<Setting> setting);

        void
        Reset() {
            this->m_max_valid_range_ = std::numeric_limits<Dtype>::min();
        }

        [[nodiscard]] std::pair<long, long>
        GetFrameShape() const override;

        [[nodiscard]] bool
        ComputeFrameCoords(const Vector3 &xyz_frame, Dtype &dist, Vector2 &frame_coords)
            const override;

        void
        UpdateRanges(
            const Eigen::Ref<const Matrix3> &rotation,
            const Eigen::Ref<const Vector3> &translation,
            MatrixX ranges) override;

        [[nodiscard]] std::shared_ptr<const Setting>
        GetSetting() const {
            return m_setting_;
        }

        [[nodiscard]] long
        GetNumAzimuthLines() const {
            return this->m_frame_coords_.rows();
        }

        [[nodiscard]] long
        GetNumElevationLines() const {
            return this->m_frame_coords_.cols();
        }

        [[nodiscard]] MatrixX
        PointCloudToRanges(
            const Matrix3 &rotation,
            const Vector3 &translation,
            const Eigen::Ref<const Matrix3X> &points,
            bool are_local) const override;

        [[nodiscard]] bool
        operator==(const Super &other) const override;

        [[nodiscard]] bool
        Write(std::ostream &s) const override;

        [[nodiscard]] bool
        Read(std::istream &s) override;
    };

    using LidarFrame3Dd = LidarFrame3D<double>;
    using LidarFrame3Df = LidarFrame3D<float>;

    extern template class LidarFrame3D<double>;
    extern template class LidarFrame3D<float>;

}  // namespace erl::geometry