Measure/include/tool/static_tool.hpp

//
// Created by zx on 2021/5/11.
//
#pragma once

#include "defines.hpp"
#include <fstream>
#include <fcntl.h>
#include <vector>
#include <pcl/io/pcd_io.h>

static bool string2point(const std::string &str, pcl::PointXYZ &point) {
    std::istringstream iss;
    iss.str(str);
    float value;
    float data[3];
    for (int i = 0; i < 3; ++i) {
        if (iss >> value) {
            data[i] = value;
        } else {
            return false;
        }
    }
    point.x = data[0];
    point.y = data[1];
    point.z = data[2];
    return true;
}

static bool ReadTxtCloud(const std::string &file, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
    DLOG(INFO) << "Load point data form file: " << file;
    std::ifstream fin(file.c_str());
    if (fin.bad()) {
        LOG(WARNING) << "File " << file << " don't exist.";
        return false;
    }
    const int line_length = 64;
    char str[line_length] = {0};
    while (fin.getline(str, line_length)) {
        pcl::PointXYZ point{};
        if (string2point(str, point)) {
            cloud->push_back(point);
        }
    }
    DLOG(INFO) << "Load point complete, get point number: " << cloud->size();
    return true;
}

static bool WriteTxtCloud(const std::string &file, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
    DLOG(INFO) << "begain push point data to " << file;
    std::fstream fileTxtCloud;
    fileTxtCloud.open(file, std::ios_base::trunc | std::ios::in | std::ios::out);
    for (auto &point: *cloud) {
        fileTxtCloud << point.x << " " << point.y << " " << point.z << std::endl;
    }
    fileTxtCloud.close();
    DLOG(INFO) << "Push point complete, push point number: " << cloud->size();
    return true;
}

static bool WriteTxtCloud(const std::string &file, pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud) {
    DLOG(INFO) << "begain push point data to " << file;
    std::fstream fileTxtCloud;
    fileTxtCloud.open(file, std::ios_base::trunc | std::ios::in | std::ios::out);
    for (auto &point: *cloud) {
        fileTxtCloud << point.x << " " << point.y << " " << point.z << std::endl;
    }
    fileTxtCloud.close();
    DLOG(INFO) << "Push point complete, push point number: " << cloud->size();
    return true;
}

static bool WriteTxtCloud(const std::string &file, pcl::PointCloud<pcl::PointALL>::Ptr &cloud) {
    DLOG(INFO) << "begain push point data to " << file;
    std::fstream fileTxtCloud;
    fileTxtCloud.open(file, std::ios_base::trunc | std::ios::in | std::ios::out);
    for (auto &point: *cloud) {
        fileTxtCloud << point.x << " " << point.y << " " << point.z << " " << point.intensity << std::endl;
    }
    fileTxtCloud.close();
    DLOG(INFO) << "Push point complete, push point number: " << cloud->size();
    return true;
}

template <typename T>
static int WritePCDCloud(const std::string &file, boost::shared_ptr<pcl::PointCloud<T>> &cloud) {
//static int WritePCDCloud(const std::string &file, boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> &cloud) {
    return pcl::io::savePCDFile(file, *cloud);
}
#include <Eigen/Core>

// 计算均方误差
static bool computer_var(std::vector<double> data, double &var) {
    if (data.empty())
        return false;
    Eigen::VectorXd dis_vec(data.size());
    for (int i = 0; i < data.size(); ++i) {
        dis_vec[i] = data[i];
    }
    double mean = dis_vec.mean();
    Eigen::VectorXd mean_vec(data.size());
    Eigen::VectorXd mat = dis_vec - (mean_vec.setOnes() * mean);
    Eigen::MatrixXd result = (mat.transpose()) * mat;
    var = sqrt(result(0) / double(data.size()));
    return true;
}

#include <opencv2/opencv.hpp>

static double distance(cv::Point2f p1, cv::Point2f p2) {
    return sqrt(pow(p1.x - p2.x, 2.0) + pow(p1.y - p2.y, 2.0));
}

#include <google/protobuf/message.h>
#include <google/protobuf/text_format.h>
#include <google/protobuf/io/zero_copy_stream_impl.h>

//读取protobuf 配置文件，转化为protobuf参数形式
//input：	prototxt_path ：prototxt文件路径
//ouput：	parameter: protobuf参数，这里是消息基类，实际调用时传入对应的子类即可。
static bool read_proto_param(std::string prototxt_path, ::google::protobuf::Message &protobuf_parameter) {
    int fd = open(prototxt_path.c_str(), O_RDONLY);
    if (fd == -1) return false;
    google::protobuf::io::FileInputStream *input = new google::protobuf::io::FileInputStream(fd);
    bool ret = google::protobuf::TextFormat::Parse(input, &protobuf_parameter);
    delete input;
    close(fd);
    return ret;
}

template <typename T>
static T mean(std::vector<T> &values, int erase = 0) {
    T m = 0;
    if (erase) {
        if (erase * 2 > values.size()) {
            return 0;
        }

        sort(values.begin(), values.end());
        for (int i = 0; i < erase; i++) {
            values.erase(values.begin());
            values.erase(values.end() - 1);
        }
    }

    for (auto &value: values) {
        m += value;
    }

    return m / values.size();
}

template <typename T>
static T variance(const T &mean, std::vector<T> &values) {
    T var = 0;
    for (auto &value: values) {
        var += sqrt((value - mean) * (value - mean));
    }
    return var / values.size();
}

template <typename T>
static T MSE(std::vector<T> &values, int erase = 1, float radio = 1) {
    if (erase) {
        if (erase * 2 > values.size()) {
            return 0;
        }

        sort(values.begin(), values.end());
        for (int i = 0; i < erase; i++) {
            values.erase(values.begin());
            values.erase(values.end() - 1);
        }
    }
    T mse = mean(values);
    T var = variance(mse, values);

    int mse_num = 0;
    mse = 0;
    for (auto &value: values) {
        if (fabs(value - var) < radio * var) {
            mse += value;
            mse_num++;
        }
    }
    return mse / mse_num;
}