IndoorPointCloudViewer/basic_methods.cpp at master · peterliu502/IndoorPointCloudViewer · 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
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
#include <pcl/point_types.h>
#include <pcl/io/ply_io.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <vector>
#include <map>
#include "./voxeliser/Workflow.h"

class Cell{
private:
    double value = 0;
public:
    std::vector<pcl::PointXYZI*> pt_vec = std::vector<pcl::PointXYZI*>();
    void set_val(double val) {value = val;}
    double get_val() const {return value;}
};

class Grid{
private:
    unsigned int col_num;
    unsigned int row_num;
    double x_min;
    double y_min;
    double size;
    std::map<std::pair<unsigned int, unsigned int>, Cell> grid_cell;

public:
    Grid(pcl::PointCloud<pcl::PointXYZI>::Ptr& pts, double _size){
        size = _size;
        double x_max, y_max;
        for(int i = 0; i < pts->size(); ++i){
            if(i == 0){
                x_min = pts->points[i].x;
                x_max = pts->points[i].x;
                y_min = pts->points[i].y;
                y_max = pts->points[i].y;
            }
            else{
                if(pts->points[i].x > x_max) x_max = pts->points[i].x;
                else if (pts->points[i].x < x_min) x_min = pts->points[i].x;
                if(pts->points[i].y > y_max) y_max = pts->points[i].y;
                else if (pts->points[i].y < y_min) y_min = pts->points[i].y;
            }
        }

        col_num = int((x_max - x_min) / size) + 1;
        row_num = int((y_max - y_min) / size) + 1;

        for (auto &pt: pts->points){
            unsigned int col = int((pt.x - x_min) / size);
            unsigned int row = int((pt.y - y_min) / size);
            grid_cell[{row, col}].pt_vec.push_back(&pt);
        }
    }

    unsigned int get_col_num() const{return col_num;}

    unsigned int get_row_num() const{return row_num;}

    double get_x_min() const{return x_min;}

    double get_y_min() const{return y_min;}

    double get_size() const{return size;}

    Cell* find_cell_by_coord(double x, double y){
        unsigned int col = int((x - x_min) / size);
        unsigned int row = int((y - y_min) / size);
        if (col >= col_num || row >= row_num) assert(false);
        return &grid_cell[{row, col}];
    }

    Cell* find_cell_by_idx(unsigned int row, unsigned int col){
        if (col >= col_num || row >= row_num) assert(false);
        return &grid_cell[{row, col}];
    }

    std::pair<double, double> get_cell_center(unsigned int row, unsigned int col) const{
        double cc_x = x_min + 0.5 * size + col * size;
        double cc_y = y_min + 0.5 * size + row * size;
        return std::pair<double, double>{cc_x, cc_y};
    }

    std::vector<double> get_cell_center_3d(unsigned int row, unsigned int col) {
        if (col >= col_num || row >= row_num) assert(false);
        double cc_x = x_min + 0.5 * size + col * size;
        double cc_y = y_min + 0.5 * size + row * size;
        return std::vector<double>{cc_x, cc_y, grid_cell[{row, col}].get_val()};
    }
};

template<typename t>
double max_vec_elm(std::vector<t> vec){
    double max_val;
    for (int i = 0; i < vec.size(); ++i) {
        if (i == 0) max_val = vec[0];
        else if (vec[i] > max_val) max_val = vec[i];
    }
    return max_val;
}

template<typename t>
double min_vec_elm(std::vector<t> vec){
    double min_val;
    for (int i = 0; i < vec.size(); ++i) {
        if (i == 0) min_val = vec[0];
        else if (vec[i] < min_val) min_val = vec[i];
    }
    return min_val;
}

template<typename t>
double avg_vec_elm(std::vector<t> vec){
    double avg_val = 0;
    for (int i = 0; i < vec.size(); ++i) {
        avg_val += vec[i];
    }
    return avg_val / vec.size();
}

template<typename t>
double mean_sqr_err(std::vector<t> vec){
    double avg_val = avg_vec_elm(vec);
    double err_val = 0;
    for (int i = 0; i < vec.size(); ++i) {
        err_val += abs(avg_val - vec[i]);
    }
    return err_val / vec.size();
}