//
// Created by ivan on 17.01.2022.
//

#ifndef ORB_SLAM3_PROCESSING_FUNCTIONS_H
#define ORB_SLAM3_PROCESSING_FUNCTIONS_H

#include <vector>
#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>

using namespace std;

cv::Mat& extract_rot(cv::Mat& rot, const cv::Mat& trans);

vector<float> operator + (vector<float>& lhs, vector<float>& rhs);

vector<float> operator + (vector<float>& lhs, float rhs);

vector<float> operator - (vector<float>& lhs, float rhs);

vector<float> operator - (vector<float>& arg);

vector<float> operator - (vector<float>& lhs, vector<float>& rhs);

vector<float> operator * (vector<float>& lhs, float rhs);

bool operator == (vector<float>& lhs, float rhs);

void calc_avg_velocity(const vector<vector<float>>& recent_velocities, vector<float>& avg_velocity);

void calculate_velocity(const vector<vector<float>>& recent_values, vector<float>& avg_velocity);

void process_lost_euler(vector<float>& euler_prev, vector<float>& euler_now, int recent_values_desired_length,
                        const vector<vector<float>>& recent_values, vector<float>& avg_velocity);

void fill_recent_values(const vector<float>& value, vector<vector<float>>& recent_values, int length=5);

void process_euler(std::vector<float>& euler_prev, std::vector<float>& euler_now, std::vector<float>& skew_angle,
                   std::vector<int>& all_maps_id, int& prevID, int& currID, vector<float>& avg_velocity);

vector<float>& extract_translation(vector<float>& transl, const cv::Mat& tcw);

bool isMatEmpty(const cv::Mat& mat);

bool isMatEye(const cv::Mat& mat);









#endif //ORB_SLAM3_PROCESSING_FUNCTIONS_H