提交坐标变化代码

coordinate/coordinate_convert.cpp

+//
+// Created by nickfox on 21-1-25.
+//
+#include <cmath>
+#include <algorithm>
+
+#include "coordinate_convert.h"
+
+void CoordinateConvert::Init(float horn_r, double rx, double ry, float horn_v, double vx, double vy){
+    // horn = (270-90)/180 * 3.1415926;
+    m_HornR = (horn_r-90)/180.0 * 3.1415926;  // radar
+    m_RadarWorld.push_back(rx);
+    m_RadarWorld.push_back(ry);
+
+    m_HornV = (horn_v)/180.0 * 3.1415926;   // vision
+    m_CameraWorld.push_back(vx);
+    m_CameraWorld.push_back(vy);
+    m_CameraWorld.push_back(0);
+
+}
+
+int32_t CoordinateConvert::Dev2WGS84(const jfx::Array & radarObj,
+                                       jfx::Array & result, bool flag) {
+    double objtmp_x = 0.0;
+    double objtmp_y = 0.0;
+    if(flag == true){
+        //radar
+        objtmp_x = radarObj[0] * cos(m_HornR) + radarObj[1] * sin(m_HornR);
+        objtmp_y = radarObj[1] * cos(m_HornR) - radarObj[0] * sin(m_HornR);
+    }else{
+        // vision
+        //printf("vision-input: %f, %f\n", radarObj[0], radarObj[1]);
+       objtmp_x = radarObj[0] * cos(m_HornV) + radarObj[1] * sin(m_HornV);
+       objtmp_y = radarObj[1] * cos(m_HornV) - radarObj[0] * sin(m_HornV);
+    }
+
+    double objWorld_x, objWorld_y;
+    if (flag == true) {
+        objWorld_x = m_RadarWorld[0] + objtmp_x;
+        objWorld_y = m_RadarWorld[1] + objtmp_y;
+    } else {
+        objWorld_x = m_CameraWorld[0] + objtmp_x;
+        objWorld_y = m_CameraWorld[1] + objtmp_y;
+    }
+
+    jfx::Array objWorld = {objWorld_x, objWorld_y, 1};
+    result = jfx::Inverse(objWorld);
+    return 0;
+}
+
+int32_t CoordinateConvert::Radar2WGS84(const jfx::Array & radarObj,
+                                       const jfx::Array & radarWorld,
+                                       const jfx::Array & cameraWorld,
+                                       jfx::Array & result) {
+    double objWorld_x = (radarWorld[0] + cameraWorld[0])/2 + radarObj[0];
+    double objWorld_y = (radarWorld[1] + cameraWorld[1])/2 + radarObj[1];
+
+    jfx::Array objWorld = {objWorld_x, objWorld_y, 1};
+    result = jfx::Inverse(objWorld);
+    return 0;
+}
+
+int32_t CoordinateConvert::Radar2World(const jfx::Array & radarObj,
+                                       jfx::Array & result, bool flag) {
+    double objtmp_x = 0.0;
+    double objtmp_y = 0.0;
+    if(flag == true){
+        //radar
+        objtmp_x = radarObj[0] * cos(m_HornR) + radarObj[1] * sin(m_HornR);
+        objtmp_y = radarObj[1] * cos(m_HornR) - radarObj[0] * sin(m_HornR);
+    }else{
+        // vision
+        //printf("vision-input: %f, %f\n", radarObj[0], radarObj[1]);
+        objtmp_x = radarObj[0] * cos(m_HornV) + radarObj[1] * sin(m_HornV);
+        objtmp_y = radarObj[1] * cos(m_HornV) - radarObj[0] * sin(m_HornV);
+    }
+
+    result = {objtmp_x, objtmp_y, 1};
+
+    return 0;
+}
+
+#if 0
+int main() {
+    CoordinateConvert m_CoordinatCvt;
+    m_CoordinatCvt.Init();
+    std::vector<std::string> name = {
+        "1614765990.8"
+    };
+    std::vector<float> dates = {
+		//1.0442, 63.2229
+		2.664277285337448, 33.64991358388215
+    };
+
+    for (int i = 0; i < dates.size(); i+=2) {
+        jfx::Array result, obj_p;
+        //obj_p = {-2.9017153158783913, 27.35474405437708};
+        obj_p.push_back(dates[i]);
+        obj_p.push_back(dates[i+1]);
+        m_CoordinatCvt.Radar2WGS84(obj_p, m_CoordinatCvt.RadarWorld, result, false);
+        printf("%s,%.8f,%.8f\n", name[i/2].c_str(), result[0], result[1]);
+        //return 0;
+    }
+
+    // /************************************************************/
+    // g_CoordinatCvt.reset(new CoordinateConvert);
+    // g_CoordinatCvt->Init();
+    // jfx::Array result, obj_p;
+    // // double h = 4.04;
+    // // double x = sqrt(pow(obj.XPos, 2) - pow(h, 2));
+    // obj_p = {-2.9017153158783913, 27.35474405437708}; 
+    // g_CoordinatCvt->Radar2WGS84(obj_p, g_CoordinatCvt->RadarWorld, result, false);
+    // printf("result= %.8f %.8f\n", result[0], result[1]);
+    // cout << "\n-----------------------------------\n" << endl;
+    // obj_p.clear();
+    // obj_p = {27.35474405437708, 2.9017153158783913}; 
+    // g_CoordinatCvt->Radar2WGS84(obj_p, g_CoordinatCvt->RadarWorld, result, true);
+    // printf("result= %.8f %.8f\n", result[0], result[1]);
+    // return 0;
+    // /************************************************************/
+
+
+    return 0;
+}
+#endif

coordinate/coordinate_convert.h

+//
+// Created by nickfox on 21-1-25.
+//
+
+#ifndef JFX_COORDINATE_CONVERT_H
+#define JFX_COORDINATE_CONVERT_H
+
+#include <cstdint>
+#include "gaussian.h"
+
+class CoordinateConvert {
+public:
+    CoordinateConvert(){};
+    virtual ~CoordinateConvert(){};
+
+    void Init(float horn_r, double rx, double ry, float horn_v, double vx, double vy);
+
+    int32_t Dev2WGS84(const jfx::Array & radarObj,
+                        jfx::Array & result, bool flag);
+
+    int32_t Radar2WGS84(const jfx::Array & radarObj,
+                        const jfx::Array & radarWorld,
+                        const jfx::Array & cameraWorld,
+                        jfx::Array & result);
+
+    int32_t Radar2World(const jfx::Array & radarObj,
+                        jfx::Array & result, bool flag);
+
+public:
+    double m_HornR;
+    jfx::Array m_RadarWorld;
+
+    double m_HornV;
+    jfx::Array m_CameraWorld;
+};
+
+
+#endif // JFX_COORDINATE_CONVERT_H

coordinate/gaussian.cpp

+#include "gaussian.h"
+
+namespace jfx {
+
+const double ZoneOffset = 1e6;
+const int ZoneWidth = 3;
+const double EastOffset = 500000.0;
+const double EARTH_RADIUS = 6378137.0;
+
+double GetDirection1(double lng1, double lat1, double lng2, double lat2){
+
+    double radLng1 = degree2rad(lng1);
+    double radLat1 = degree2rad(lat1);
+    double radLng2 = degree2rad(lng2);
+    double radLat2 = degree2rad(lat2);
+    
+    double a = degree2rad(90 - lat2);
+    double b = degree2rad(90 - lat1);
+    double c = degree2rad(lng2- lng1);
+    double cos_c = (cos(b) * cos(a)) + (sin(b) * sin(a) * cos(c));
+    double sin_c = sqrt(1 - pow(cos_c, 2));
+    double sin_a = sin(sin(b) * sin(c) / sin_c);
+    double Azimuth = asin(sin_a) * 180 / pi;
+
+    double x = lng2 - lng1;
+    double y = lat2 - lat1;
+    if (x > 0 && y > 0)
+    {
+	return Azimuth;
+    }
+    else if (y > 0 && x < 0)
+    {
+	return 360 + Azimuth;
+    }
+    else
+    {
+	return 180 - Azimuth;
+    }
+
+}
+
+double GetDirection2(double lng1, double lat1, double lng2, double lat2){
+
+    double radLng1 = degree2rad(lng1);
+    double radLat1 = degree2rad(lat1);
+    double radLng2 = degree2rad(lng2);
+    double radLat2 = degree2rad(lat2);
+
+    double a = degree2rad(lng2 - lng1);
+    double b = degree2rad(lat2 - lat1);
+    double c = cos(radLat2);
+    double result1 = tan(a * c / b);
+    double result = atan(result1) * 180 / pi;
+
+    double x = lng2 - lng1;
+    double y = lat2 - lat1;
+    if (x > 0 && y > 0)
+    {
+	return result;
+    }
+    else if (y > 0 && x < 0)
+    {
+	return 360 + result;
+    }
+    else
+    {
+	return 180 + result;
+    }
+}
+
+double GetDistance(double lng1, double lat1, double lng2, double lat2){
+
+    double radLat1 = degree2rad(lat1);
+    double radLng1 = degree2rad(lng1);
+    double radLat2 = degree2rad(lat2);
+    double radLng2 = degree2rad(lng2);
+    double a = radLat2 - radLat1;
+    double b = radLng2 - radLng1;
+    double result = 2 * asin(sqrt(pow(sin(a / 2), 2) + cos(radLat1) * cos(radLat2) * pow(sin(b / 2), 2))) * EARTH_RADIUS;
+    return result;
+}
+
+double GetDistance_xy(double x1, double y1, double x2, double y2){
+    return sqrt(pow(x2-x1,2)+pow(y2-y1,2));
+}
+
+Array Convert(const Array& inLonLat){
+    int Zone = (int)((inLonLat[1] + 1.5) / ZoneWidth);
+    int L0 = Zone * ZoneWidth;
+
+    double dL = inLonLat[1] - L0 * 1.0;
+
+    double dLRadian = degree2rad(dL);
+    double latRadian = degree2rad(inLonLat[0]);
+
+    double X = GetX(latRadian);
+    double N = GetN(latRadian);
+
+    double t = tan(latRadian);
+    double t2 = t * t;
+    double sinB = sin(latRadian);
+    double cosB = cos(latRadian);
+    double ita2 = ep2 * cosB * cosB;
+
+    double temp = dLRadian * cosB;
+    double temp2 = temp * temp;
+    double yPart1 = 0.5 * N * temp * temp * t;
+    double yPart2 = N * t * (5.0 - t2 + 9.0 * ita2 + 4.0 * ita2 * ita2) * temp2 * temp2 / 24.0;
+    double yPart3 = temp2 * temp2 * temp2 * N * t * (61.0 - 58.0 * t2 + t2 * t2) / 720.0;
+    double y = X + yPart1 + yPart2 + yPart3;
+
+    double xPart1 = N * temp;
+    double xPart2 = (1.0 - t2 + ita2) * N * temp * temp2 / 6.0;
+    double xPart3 = (5.0 - 18.0 * t2 + t2 * t2 + 14.0 * ita2 - 58.0 * ita2 * t2) * N * temp * temp2 * temp2 / 120.0;
+
+    double x = Zone * 1.0 * ZoneOffset * 1.0 + EastOffset + xPart1 + xPart2 + xPart3;
+    double h = inLonLat[2];
+
+
+    return Array({x,y,h});
+}
+
+Array Inverse(const Array& inXY){
+    double Zone = (int)(inXY[0] / ZoneOffset);
+
+    double L0 = Zone * ZoneWidth;
+    double L0_radian = degree2rad(L0);
+
+    double  X0 = Zone * ZoneOffset + EastOffset;
+    double Y0 = 0;
+
+    double x = inXY[0] - X0;
+    double y = inXY[1] - Y0;
+
+    double Br = GetLatByX(y);
+
+
+    double cosB = cos(Br);
+    double secB = 1.0 / cosB;
+    double ita2 = ep2 * cosB * cosB;
+    double t = tan(Br);
+    double t2 = t * t;
+    double V = GetV(Br);
+    double V2 = V * V;
+    double N = c / V;
+    double M = N / V / V;
+    double D = x / N;
+
+
+    double tmp3 = (1.0 + 2.0 * t2 + ita2) * D * D * D / 6.0;
+    double tmp5 = (5.0 + 28.0 * t2 + 24.0 * t2 * t2 + 6.0 * ita2 + 8.0 * ita2 * t2) * D * D * D * D * D / 120.0;
+    double l = secB * (D - tmp3 + tmp5);
+    double L_radian = L0_radian + l;
+    double tmp2 = D * D / 2.0;
+    double tmp4 = (5.0 + 3.0 * t2 + ita2 - 9.0 * ita2 * t2) * D * D * D * D / 24.0;
+    double tmp6 = (61.0 + 90.0 * t2 + 45.0 * t2 * t2) * D * D * D * D * D * D / 720.0;
+    double B_radian = Br - t * V2 * (tmp2 - tmp4 + tmp6);
+
+    double B = rad2degree(B_radian);
+    double L = rad2degree(L_radian);
+    return Array({B,L,inXY[2]});
+}
+
+} // namespace jfx
\ No newline at end of file

coordinate/gaussian.h

+#ifndef JFX_GAUSSIAN_H
+#define JFX_GAUSSIAN_H
+
+#include "rotations.h"
+#include "wgs84.h"
+
+namespace jfx {
+
+Array Convert(const Array& inLonLat);
+
+Array Inverse(const Array& inXY);
+
+double GetDirection1(double lng1, double lat1, double lng2, double lat2);
+
+double GetDirection2(double lng1, double lat1, double lng2, double lat2);
+
+double GetDistance_deg(double lng1, double lat1, double lng2, double lat2);
+
+double GetDistance_xy(double x1, double y1, double x2, double y2);
+
+} // namespace jfx
+
+#endif // JFX_GAUSSIAN_H

coordinate/ipm.cpp

+#include "ipm.h"
+
+using namespace std;
+
+namespace jfx {
+
+// calculating world_xyz -> world_xy
+//      return [world_x, world_y, 1]
+vector<float> transform(vector<float> M, vector<int>& points, int point_num) {
+    vector<float> new_xy(point_num * 3, 1.0);       // new_xy[2, :]  = 1.
+    int idx = 0;
+
+    // z = M[2, :] * POINTS
+    vector<float> z;
+    for (int j = 0; j < point_num; j++) {
+        int T_start = j * 2;
+        float r = 0.0;
+        for (int k = 0; k < 3; k++) {
+            if (k == 2)         // Cause' points[3, :] = 1.
+                r += M[6+k];
+            else
+                r += M[6+k] * points[T_start+k];
+        }
+        z.push_back(r);
+    }
+
+    // new_xy[:2, :] = M[:2, :] * POINTS / z
+    for (int i = 0; i < 2; i++) {
+        int M_start = i * 3;
+        for (int j = 0; j < point_num; j++) {
+            int T_start = j * 2;
+            float r = 0.0;
+            for (int k = 0; k < 3; k++) {
+                if (k == 2)         // Cause' points[3, :] = 1.
+                    r += M[M_start+k];
+                else
+                    r += M[M_start+k] * points[T_start+k];
+            }
+            new_xy[idx++] = r/z[j];
+        }
+    }
+    
+    return new_xy;
+}
+
+float courseAngleAccordingLatLon(float lat_a, float lng_a, float lat_b, float lng_b) {
+	float y = sin(lng_b - lng_a) * cos(lat_b);
+	float x = cos(lat_a) * sin(lat_b) - sin(lat_a) * cos(lat_b) * cos(lng_b - lng_a);
+	float bearing = atan2(y, x);
+
+	// debugger;
+	bearing = 180 * bearing / M_PI;
+	if (bearing < 0) {
+		bearing = bearing + 360;
+	}
+	return bearing;
+}
+
+
+float courseAngle(float x1, float y1, float x2, float y2) {
+	bool debug = false;
+	float delta_y = y2 - y1;
+	float delta_x = x2 - x1;
+#if 0
+	// 4.52702582618177
+	delta_x = 2.664277285337448;
+	delta_y = 33.64991358388215;
+	x1 = 0; y1 = 0; x2 = delta_x; y2 = delta_y;
+#endif
+#if 0
+	float bearing = atan2(delta_y, delta_x);
+	if (debug) printf("0 - theta = %f, delta_y = %f, delta_x = %f\n", bearing, delta_y, delta_x);
+	// debugger;
+	bearing = 180 * bearing / M_PI;
+	if (bearing < 0) {
+		bearing = bearing + 360;
+	}
+#else
+	float bearing = atan(delta_y / delta_x);
+	bearing = 180 * bearing / M_PI;
+#endif
+	if (debug) printf("1 - theta = %f, delta_y = %f, delta_x = %f\n", bearing, delta_y, delta_x);
+	if (x2 > x1 && y2 > y1) bearing = 90 - bearing;
+	else if (x2 > x1 && y2 < y1) bearing += 90;
+	else if (x2 < x1 && y2 > y1) bearing += 270;
+	else if (x2 < x1 && y2 < y1) bearing = 270 - bearing;
+	else if (x2 == x1 && y2 < y1) bearing = 180;
+	else if (x2 < x1 && y2 == y1) bearing = 270;
+	else if (x2 > x1 && y2 == y1) bearing = 90;
+	if (debug) printf("2 - theta = %f, delta_y = %f, delta_x = %f\n", bearing, delta_y, delta_x);
+	return bearing;
+}
+
+float getYYaw(int row, int col, cv::Mat cameraMatrix, cv::Mat distCoeffs, vector<float> M) {
+	// 2.635
+	int col_mid = int(col/2), row_mid = int(row/2);
+	std::vector<cv::Point2f> outputUndistortedPoints;
+	std::vector<cv::Point2f> inputDistortedPoints1{cv::Point2f(col_mid, row-100)};
+	std::vector<cv::Point2f> inputDistortedPoints2{cv::Point2f(col_mid, row-500)};
+	std::vector<cv::Point2f> inputDistortedPoints3{cv::Point2f(col_mid+10, row-500)};
+
+	cv::undistortPoints(inputDistortedPoints1, outputUndistortedPoints, cameraMatrix, distCoeffs, cv::noArray(), cameraMatrix);
+	vector<int> points1 = {(int)outputUndistortedPoints[0].x, (int)outputUndistortedPoints[0].y};
+	vector<float> result1 = transform(M, points1, 1);
+	cv::undistortPoints(inputDistortedPoints2, outputUndistortedPoints, cameraMatrix, distCoeffs, cv::noArray(), cameraMatrix);
+	vector<int> points2 = {(int)outputUndistortedPoints[0].x, (int)outputUndistortedPoints[0].y};
+	vector<float> result2 = transform(M, points2, 1);
+	cv::undistortPoints(inputDistortedPoints3, outputUndistortedPoints, cameraMatrix, distCoeffs, cv::noArray(), cameraMatrix);
+	vector<int> points3 = {(int)outputUndistortedPoints[0].x, (int)outputUndistortedPoints[0].y};
+	vector<float> result3 = transform(M, points3, 1);
+
+	float angle1 = courseAngle(result1[0], result1[1], result2[0], result2[1]);
+	float angle2 = courseAngle(result1[0], result1[1], result3[0], result3[1]);
+	//printf("Angle = %f, %f, %f, %f, %f\n", angle1, result1[0], result1[1], result2[0], result2[1]);
+	//printf("Angle = %f, %f, %f, %f, %f\n", angle2, result1[0], result1[1], result3[0], result3[1]);
+
+	return angle2 - angle1;
+}
+
+} // namespace jfx
\ No newline at end of file

coordinate/ipm.h

+#ifndef JFX_IPM_H
+#define JFX_IPM_H
+
+#include <cmath>
+#include <string>
+#include <vector>
+#include <iostream>
+
+#include <sys/time.h>
+#include <opencv2/opencv.hpp>
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+namespace jfx {
+
+std::vector<float> transform(std::vector<float> M, std::vector<int>& points, int point_num);
+
+float courseAngle(float x1, float y1, float x2, float y2);
+
+float getYYaw(int row, int col, cv::Mat cameraMatrix, cv::Mat distCoeffs, std::vector<float> M);
+
+} // namespace jfx
+#endif

coordinate/matrix.h

+#ifndef JFX_MATRIX_H
+#define JFX_MATRIX_H
+
+#include <algorithm>
+#include <assert.h>
+#include <deque>
+#include <iomanip>
+#include <iostream>
+#include <map>
+#include <math.h>
+#include <sstream>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace jfx {
+
+using std::vector;
+using std::string;
+using std::cout;
+using std::endl;
+using std::ios;
+using std::setprecision;
+
+const string StrNone="None";
+const double pi=3.1415926535898;
+class matrix;
+class Array;
+
+matrix diag(const vector<double>& f);
+Array zeros(const int& x);
+matrix zeros(const int& x,const int& y);
+vector<vector<vector<double>>> zeros(const int& x,const int& y,const int& z);
+matrix eye(const int& k);
+double linalg_norm(const Array& t);
+
+
+class dict{
+public:
+    bool empty;
+    double time;
+    vector<double > arr;
+    std::unordered_map<int,vector<double>> mp;
+    vector<vector<double>> acc;
+    vector<vector<double>> gryo;
+    dict():empty(true){time = 0;}
+    dict (const vector<double>& temp):arr(temp),empty(true){time = 0;}
+    dict (const Array& temp);
+    vector<double>& operator[](int k){
+        return mp[k];
+    }
+    bool is_in(int k)const{
+        for( auto i = mp.begin(); i != mp.end() ; i++)
+            if( (*i).first == k) return true;
+        return false;
+    }
+    void clear();
+    void acc_push_back(const Array& tem);
+    void gryo_push_back(const Array& tem);
+    void print();
+};
+
+class Array{
+public:
+    vector<double> arr;
+    Array(){}
+    Array( const vector<double>& temp ):arr( temp ){}
+    Array( const std::initializer_list<double>& temp);
+
+    Array(const string& ){clear();}
+    Array(const dict& temp):arr(temp.arr){}
+    int size()const{return arr.size();}
+    void clear(){arr.clear();}
+    void resize( int k );
+    void print()const;
+    bool empty()const{return arr.empty();}
+    void push_back( double num) { arr.push_back(num);}
+
+    Array& operator=( const string& ){clear();return *this;}
+    Array& operator=( const vector<double>& temp ){arr=temp;return *this;}
+    Array& operator=( const dict& temp){ arr = temp.arr;return *this;}
+    Array& operator=( const matrix& temp);
+    Array& operator=( const std::initializer_list<double>& temp);
+    Array& operator=( const Array& temp);
+
+    Array operator+( const vector<double>& temp )const;
+    Array operator+( const Array& temp )const;
+
+    Array operator-( const vector<double>& temp )const ;
+    Array operator-( const Array& temp )const;
+
+    Array operator*( const matrix& temp )const;
+    double operator*( const Array& temp) const;
+    Array operator*( const double& k )const;
+
+    Array operator/( const double& k )const;
+
+    double& operator[]( int k ) { return arr[k]; }
+    const double& operator[] ( int k )const { return arr[k]; }
+
+    bool operator==(const string&) const{ return arr.empty(); }
+    bool operator!=(const string&) const {return !arr.empty(); }
+};
+
+/////////////////////////////////////////////////////////////////////////////
+class matrix {
+public:
+    vector<vector<double>> arr;
+
+    matrix() {}
+
+    matrix(const vector<vector<double>>& tem) : arr(tem) {}
+
+    matrix(const vector<double>& tem){arr.push_back(tem);}
+    
+    matrix( const std::initializer_list<std::initializer_list<double>>& temp);
+
+    matrix(const matrix& m) { *this = m; }
+
+    matrix(const string& tem){}
+
+    matrix(Array& tem) { arr.push_back(tem.arr); }
+
+    matrix(Array&& tem) { arr.push_back(tem.arr); }
+    
+    matrix T()const;
+    matrix Inv()const;
+
+    matrix operator*(const matrix& m)const;
+    matrix operator*(const double& k)const;
+    Array operator*(const Array& m)const;
+    matrix operator*(const vector<vector<double>>& m)const;
+    Array operator*(const vector<double>& m)const;
+
+    matrix operator+(const matrix& m)const;
+
+    matrix operator-(const matrix& m)const;
+
+    matrix operator-(const vector<double>& m)const;
+
+    matrix operator/(const double& k)const;
+
+    matrix& operator/=(const double& k);
+
+    matrix& operator+=(const matrix& m);
+
+    matrix& operator-=(const matrix& m);
+
+    matrix& operator=(const vector<double>& m);
+
+    matrix& operator=(const vector<vector<double>>& m);
+
+    matrix& operator=(const string& m);
+
+    matrix& operator=(const Array& m);
+
+    bool operator==(const string& m)const;
+
+    bool operator!=(const string& m)const;
+
+    const vector<double >& operator[](int row)const {return arr[row];}
+
+    vector<double >&operator[](int row){return arr[row];}
+
+    void swap_row(int row1,int row2);
+
+    bool is_square() const{ return (!(empty()) && rows() == cols());}
+
+    void resize(int rows,int cols);
+
+    bool empty()const{return arr.empty();}
+
+    int cols()const {return rows()?(arr[0].size()):0;}
+
+    int rows()const {return arr.size();}
+
+    int len_shape()const;
+
+    bool push_back(const vector<double>& v);
+
+    void reshape(int x,int y);
+
+    void clear();
+
+    void print()const;
+};
+
+class CirQueue
+{
+public:
+    CirQueue(double items);
+    virtual ~CirQueue();
+    bool EnQueue(double data);
+    bool DeQueue(double* pRecvData=nullptr);
+    void DeTraverse();
+    int GetSize();
+    bool IsFull();
+    bool IsEmpty();
+private:
+    double* pArr;
+    int head,tail;
+    int length;
+};
+
+} // namespace jfx
+
+#endif // JFX_MATRIX_H
\ No newline at end of file

coordinate/rotations.cpp

+#include "rotations.h"
+
+namespace jfx {
+
+matrix skew_symmetric(const Array& v)
+{
+    return matrix({ { 0, -v[2], v[1] },
+        { v[2], 0, -v[0] },
+        { -v[1], v[0], 0 } });
+}
+
+double wraps2pi_pi(double& rad)
+{
+    while (rad > pi)
+        rad -= 2 * pi;
+    while (rad < (-pi))
+        rad += 2 * pi;
+    return rad;
+}
+
+Quaternion::Quaternion(const Array& axis_angle, const Array& euler, double w, double x, double y, double z)
+{
+    if (axis_angle == StrNone && euler == StrNone) {
+        this->w = w;
+        this->x = x;
+        this->y = y;
+        this->z = z;
+    } else if (axis_angle != StrNone && euler != StrNone) {
+        cout << "Only one of axis_angle and euler can be specified." << endl;
+        exit(1);
+    } else if (axis_angle != StrNone) {
+        if (axis_angle.size() == 3) {
+            double norm = linalg_norm(axis_angle);
+            this->w = cos(norm / 2.0);
+            if (norm < 1e-10) {
+                this->x = 0;
+                this->y = 0;
+                this->z = 0;
+            } else {
+                Array imag = axis_angle / norm * sin(norm / 2.0);
+                this->x = imag[0];
+                this->y = imag[1];
+                this->z = imag[2];
+            }
+        } else {
+            cout << "axis_angle must be list or np.ndarray with length 3." << endl;
+            exit(1);
+        }
+    } else {
+        double roll = euler[0], pitch = euler[1], yaw = euler[2];
+        double cy, sy, cr, sr, cp, sp;
+        cy = cos(yaw * 0.5);
+        sy = sin(yaw * 0.5);
+        cr = cos(roll * 0.5);
+        sr = sin(roll * 0.5);
+        cp = cos(pitch * 0.5);
+        sp = sin(pitch * 0.5);
+        //# static frame
+        this->w = cr * cp * cy + sr * sp * sy;
+        this->x = sr * cp * cy - cr * sp * sy;
+        this->y = cr * sp * cy + sr * cp * sy;
+        this->z = cr * cp * sy - sr * sp * cy;
+    }
+}
+
+string Quaternion::_repr_()
+{
+    std::ostringstream os;
+    string s;
+    os << setprecision(2) << std::setw(10) << std::setiosflags(ios::fixed) << "Quaternion (wxyz): [" << this->w << "," << this->x << "," << this->y << "," << this->z << "]\n";
+    s = os.str();
+    return s;
+}
+
+matrix Quaternion::to_mat()
+{
+    Array v({ this->x, this->y, this->z });
+    matrix v_M = v;
+    matrix vTv = v_M.T() * v_M;
+    double tem = this->w * this->w - v * v;
+    matrix ans = eye(3) * tem + vTv * 2 + skew_symmetric(v) * 2.0 * this->w;
+    return ans;
+}
+
+Array Quaternion::to_euler()
+{
+    double roll = atan2(2 * (this->w * this->x + this->y * this->z), 1 - 2 * (this->x * this->x + this->y * this->y));
+    double pitch = asin(2 * (this->w * this->y - this->z * this->x));
+    double yaw = atan2(2 * (this->w * this->z + this->x * this->y), 1 - 2 * (this->y * this->y + this->z * this->z));
+    return Array({ roll, pitch, yaw });
+}
+
+Array Quaternion::to_numpy()
+{
+    return Array({ this->w, this->x, this->y, this->z });
+}
+
+Quaternion Quaternion::normalize()
+{
+    double norm = linalg_norm(Array({ this->w, this->x, this->y, this->z }));
+    return Quaternion(StrNone, StrNone, this->w / norm, this->x / norm, this->y / norm, this->z / norm);
+}
+
+Quaternion Quaternion::to_reverse()
+{
+    return Quaternion(StrNone, StrNone, w, -x, -y, -z);
+}
+
+vector<double> Quaternion::quat_outer_mult(const Array& q)
+{
+    Array v({ this->x, this->y, this->z }); //1*3的矩阵
+    //构造sum_term矩阵
+    matrix sum_term = zeros(4, 4);
+    for (int i = 1, j = 0; i < 4; i++, j++) {
+        sum_term[0][i] = -v[j];
+        sum_term[i][0] = v[j];
+    }
+
+    matrix sk = skew_symmetric(v);
+    for (int i = 1; i < 4; i++)
+        for (int j = 1; j < 4; j++)
+            sum_term[i][j] = -sk[i - 1][j - 1];
+
+    matrix sigma = eye(4) * (this->w) + sum_term;
+    Array quat_np = sigma * q;
+
+    return quat_np.arr;
+}
+
+double rad2degree(double rad)
+{
+    return rad * 180.0 / pi;
+}
+
+Array rad2degree(const Array& list)
+{
+    Array newlist = zeros(list.size());
+    int len = list.size();
+    for (int i = 0; i < len; i++)
+        newlist[i] = list[i] * 180.0 / pi;
+    return newlist;
+}
+
+double degree2rad(double angle)
+{
+    return angle * pi / 180.0;
+}
+
+Array degree2rad(const Array& list)
+{
+    Array newlist = zeros(list.size());
+    int len = list.size();
+    for (int i = 0; i < len; i++)
+        newlist[i] = list[i] * pi / 180.0;
+    return newlist;
+}
+
+Array euler2quat(const Array& eulers)
+{
+    double gamma = eulers[0], theta = eulers[1], psi = eulers[2];
+
+    double q0 = cos(gamma / 2.0) * cos(theta / 2.0) * cos(psi / 2.0) + sin(gamma / 2.0) * sin(theta / 2.0) * sin(psi / 2.0);
+    double q1 = sin(gamma / 2) * cos(theta / 2) * cos(psi / 2) - cos(gamma / 2) * sin(theta / 2) * sin(psi / 2);
+    double q2 = cos(gamma / 2) * sin(theta / 2) * cos(psi / 2) + sin(gamma / 2) * cos(theta / 2) * sin(psi / 2);
+    double q3 = cos(gamma / 2) * cos(theta / 2) * sin(psi / 2) - sin(gamma / 2) * sin(theta / 2) * cos(psi / 2);
+
+    Array quats({ q0, q1, q2, q3 });
+    return quats;
+}
+
+Array quat2euler(const Array& quats)
+{
+
+    double q0 = quats[0], q1 = quats[1], q2 = quats[2], q3 = quats[3];
+
+    double gamma = atan2(2.0 * (q0 * q1 + q2 * q3), 1.0 - 2.0 * (q1 * q1 + q2 * q2));
+    double theta = asin(2.0 * (q0 * q2 - q3 * q1));
+    double psi = atan2(2.0 * (q0 * q3 + q1 * q2), 1.0 - 2.0 * (q2 * q2 + q3 * q3));
+    Array eulers({ wraps2pi_pi(gamma), wraps2pi_pi(theta), wraps2pi_pi(psi) });
+
+    return eulers;
+}
+
+matrix quat2rot(const Array& quats)
+{
+    matrix Rs;
+    Rs.resize(3, 3);
+    double q0 = quats[0];
+    double q1 = quats[1];
+    double q2 = quats[2];
+    double q3 = quats[3];
+    Rs[0][0] = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3;
+    Rs[0][1] = 2.0 * (q1 * q2 - q0 * q3);
+    Rs[0][2] = 2.0 * (q0 * q2 + q1 * q3);
+    Rs[1][0] = 2.0 * (q1 * q2 + q0 * q3);
+    Rs[1][1] = q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3;
+    Rs[1][2] = 2.0 * (q2 * q3 - q0 * q1);
+    Rs[2][0] = 2.0 * (q1 * q3 - q0 * q2);
+    Rs[2][1] = 2.0 * (q0 * q1 + q2 * q3);
+    Rs[2][2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
+    return Rs;
+}
+
+Array rot2quat(const matrix& rots)
+{ //---待写----------------------------------------
+    Array q;
+    q.resize(4);
+    q[0] = sqrt(rots[0][0] + rots[1][1] + rots[2][2] + 1) / 2.0;
+    q[1] = (rots[1][2] - rots[2][1]) / (4.0 * q[0]);
+    q[1] = -q[1];
+    q[2] = (rots[2][0] - rots[0][2]) / (4.0 * q[0]);
+    q[2] = -q[2];
+    q[3] = (rots[0][1] - rots[1][0]) / (4.0 * q[0]);
+    q[3] = -q[3];
+    return q;
+}
+
+matrix euler2rot(const Array& eulers)
+{
+    return quat2rot(euler2quat(eulers));
+}
+
+Array rot2euler(const matrix& rots)
+{
+    return quat2euler(rot2quat(rots));
+}
+
+matrix gpseuler2rot_in_cptframe(const Array& gps_euler_degrees)
+{
+    //# R2 defines: XYZ right/forward/up -> ENU
+    double yaw = degree2rad(gps_euler_degrees[2]);
+    double pitch = degree2rad(gps_euler_degrees[1]);
+    double roll = degree2rad(gps_euler_degrees[0]);
+
+    double sinA = sin(yaw);
+    double cosA = cos(yaw);
+    matrix matA = matrix({ { cosA, sinA, 0 }, { -sinA, cosA, 0 }, { 0, 0, 1 } });
+    //matA.print();
+
+    double sinP = sin(pitch);
+    double cosP = cos(pitch);
+    matrix matP = matrix({ { 1, 0, 0 }, { 0, cosP, -sinP }, { 0, sinP, cosP } });
+    //matP.print();
+
+    double sinR = sin(roll);
+    double cosR = cos(roll);
+    matrix matR = matrix({ { cosR, 0, sinR }, { 0, 1, 0 }, { -sinR, 0, cosR } });
+
+    return matA * (matP * matR);
+}
+
+matrix gpseuler2rot(const Array& gps_euler_degrees)
+{
+    //# R1 defines: XYZ forward/left/up -> XYZ right/forward/up
+    matrix R1 = matrix({ { 0, -1, 0 },
+        { 1, 0, 0 },
+        { 0, 0, 1 } });
+    matrix R2 = gpseuler2rot_in_cptframe(gps_euler_degrees);
+
+    return R2 * R1;
+}
+
+Array quat_product(const Array& q, const Array& r)
+{
+    Array t = zeros(4);
+    t[0] = r[0] * q[0] - r[1] * q[1] - r[2] * q[2] - r[3] * q[3];
+    t[1] = r[0] * q[1] + r[1] * q[0] - r[2] * q[3] + r[3] * q[2];
+    t[2] = r[0] * q[2] + r[1] * q[3] + r[2] * q[0] - r[3] * q[1];
+    t[3] = r[0] * q[3] - r[1] * q[2] + r[2] * q[1] + r[3] * q[0];
+    return t;
+}
+
+double yaw_car2gps(double yaw_car)
+{
+    double yaw_gps;
+    yaw_gps = ((yaw_car > -180.0) && (yaw_car < -90.0)) ? (yaw_car + 270.0) : (yaw_car - 90.0);
+    yaw_gps = (yaw_gps < 0.0) ? (-yaw_gps) : (360.0 - yaw_gps);
+    return yaw_gps;
+}
+
+Array gauss2car(const Array& xyz, double yaw_gps)
+{
+    double cx, cy, cz;
+    double yaw = degree2rad(yaw_gps);
+    cx = xyz[0] * cos(yaw) - xyz[1] * sin(yaw);
+    cy = xyz[1] * cos(yaw) + xyz[0] * sin(yaw);
+    cz = xyz[2];
+    Array car_xyz = { cx, cy, cz };
+    return car_xyz;
+}
+
+} //namespace jfx
\ No newline at end of file

coordinate/rotations.h

+#ifndef JFX_ROTATIONS_H
+#define JFX_ROTATIONS_H
+
+#include "matrix.h"
+
+namespace jfx {
+
+matrix skew_symmetric(const Array& v);
+double wraps2pi_pi(double& rad);
+
+double rad2degree(double rad);
+Array rad2degree(const Array& list);
+double degree2rad(double angle);
+Array degree2rad(const Array& list);
+
+Array euler2quat(const Array& eulers);
+Array quat2euler(const Array& quats);
+matrix quat2rot(const Array& quats);
+Array rot2quat(const matrix& rots);
+matrix euler2rot(const Array& eulers);
+Array rot2euler(const matrix& rots);
+matrix gpseuler2rot_in_cptframe(const Array& gps_euler_degrees);
+matrix gpseuler2rot(const Array& gps_euler_degrees);
+Array quat_product(const Array& q, const Array& r);
+
+double yaw_car2gps(double yaw_car);
+Array gauss2car(const Array& xyz, double yaw_gps);
+
+class Quaternion {
+public:
+    double w, x, y, z;
+    Quaternion(const Array& axis_angle, const Array& euler, double w = 1, double x = 0, double y = 0, double z = 0);
+    string _repr_();
+
+    matrix to_mat();
+    Array to_euler();
+    Array to_numpy();
+    Quaternion normalize();
+    Quaternion to_reverse();
+    vector<double> quat_outer_mult(const Array& q);
+};
+
+} // namespace jfx
+
+#endif // JFX_ROTATIONS_H
\ No newline at end of file

coordinate/transform_uv.cpp

+//
+// Created by youle on 21-7-27.
+// Mail: a_fly0505@163.com
+// QQ: 785230837 WX: afly003
+// Company: juefx
+//
+#include "transform_uv.h"
+#include "ipm.h"
+
+using namespace std;
+
+namespace jfx {
+
+TransformUV::TransformUV()
+{
+}
+
+TransformUV::~TransformUV()
+{
+}
+
+// f_c_x_y: fx, cx, fy, cy
+int TransformUV::Init(std::vector<double>& f_c_x_y, std::vector<float>& transform_M, std::vector<double>& dist)
+{
+    // todo config reload
+    m_CameraMatrix = (cv::Mat_<double>(3, 3) << f_c_x_y[0], 0.0, f_c_x_y[1], 0.0, f_c_x_y[2], f_c_x_y[3], 0.0, 0.0, 1.0);
+    m_dist.clear();
+    m_dist = dist;
+    m_TransM.clear();
+    m_TransM = transform_M;
+}
+
+int32_t TransformUV::Process(double center_x, double center_y, vector<float>& r)
+{
+
+    std::vector<cv::Point2f> outputUndistortedPoints;
+    std::vector<cv::Point2f> inputDistortedPoints { cv::Point2f(center_x, center_y) };
+
+    if (m_UseCip) {
+        cv::Mat distCoeffs { m_dist };
+        cv::undistortPoints(inputDistortedPoints, outputUndistortedPoints, m_CameraMatrix, distCoeffs, cv::noArray(),
+            m_CameraMatrix);
+    } else {
+        outputUndistortedPoints = inputDistortedPoints;
+    }
+
+    vector<int> points = { (int)outputUndistortedPoints[0].x, (int)outputUndistortedPoints[0].y };
+    r = transform(m_TransM, points, points.size() / 2);
+    if (r.size() <= 0) {
+        return 1;
+    }
+    return 0;
+}
+
+} // namespace jfx
\ No newline at end of file

coordinate/transform_uv.h

+//
+// Created by youle on 21-7-27.
+// Mail: a_fly0505@163.com
+// QQ: 785230837 WX: afly003
+// Company: juefx
+//
+
+#ifndef JFX_TRANSFORM_UV_H
+#define JFX_TRANSFORM_UV_H
+
+#include <cstdint>
+#include <opencv2/opencv.hpp>
+#include <vector>
+
+namespace jfx {
+
+class TransformUV {
+public:
+    TransformUV();
+    virtual ~TransformUV();
+
+    // 传入相机外参
+    int Init(std::vector<double>& f_c_x_y, std::vector<float>& transform_M, std::vector<double>& dist);
+
+    // 根据u,v坐标计算出相对于相机的位置，单位m
+    int32_t Process(double center_x, double center_y, std::vector<float>& r);
+
+private:
+    bool m_UseCip = true;
+    std::vector<float> m_TransM;
+    std::vector<double> m_dist;
+    cv::Mat m_CameraMatrix;
+};
+
+} // namespace jfx
+#endif // JFX_TRANSFORM_UV_H

coordinate/wgs84.cpp

+#include "wgs84.h"
+
+namespace jfx {
+
+double GetN(const double& radB){
+    double cosB = cos(radB);
+    double V = sqrt(1.0 + ep2 * cosB * cosB);
+    double N = c / V;
+    return N;
+}
+
+double GetX(const double& radB){
+    double sinB = sin(radB);
+    double cosB = cos(radB);
+    double al = a0 * radB;
+    double sc = sinB * cosB;
+    double ss = sinB * sinB;
+    double X = al - sc *((a2 - a4 + a6) + (2.0 * a4 - a6 * 16.0 / 3.0) * ss + a6 * 16.0 * ss * ss /3.0);
+    return X;
+}
+double GetLatByX(const double& X){
+    double Bfi0 = X / a0;
+    double Bfi1 = 0.0;
+    bool num = true;
+    double  minAngle = pi * 1e-9;
+    double menus_minAngle = 0 - minAngle;
+    double sinB,sinB2,cosB,FBfi,deltaB;
+    while(num == true){
+        num = false;
+        sinB = sin(Bfi0);
+        sinB2 = sinB * sinB;
+        cosB = cos(Bfi0);
+        FBfi=0.0 - sinB * cosB *((a2 - a4 + a6) + sinB2 * (2.0 * a4 - 16.0 * a6 / 3.0) + sinB2 * sinB2 * a6 * 16.0 / 3.0);
+        Bfi1=(X - FBfi) / a0;
+
+        deltaB = Bfi1 - Bfi0;
+        if(deltaB < menus_minAngle || deltaB >   minAngle){
+            num = true;
+            Bfi0 = Bfi1;
+        }
+    }
+    return Bfi1;
+}
+double GetV(const double& lat){
+    double cosB = cos(lat);
+    double V = sqrt(1 + ep2 * cosB * cosB);
+    return V;
+}
+Array BLH2ECEF(const Array& ptBLH){
+    double cosB = cos( degree2rad( ptBLH[0] ) );
+    double sinB = sin( degree2rad( ptBLH[0] ) );
+    double cosL = cos( degree2rad( ptBLH[1] ) );
+    double sinL = sin( degree2rad( ptBLH[1] ) );
+
+    double N = GetN( degree2rad( ptBLH[0] ) );
+
+    double X = (N + ptBLH[2]) * cosB * cosL;
+    double Y = (N + ptBLH[2]) * cosB * sinL;
+    double Z = (N * (1 - e2) + ptBLH[2]) * sinB;
+    return Array({X,Y,Z});
+}
+Array ECEF2BLH(const Array& ptXYZ){
+    double  L_radian = atan(ptXYZ[1] / ptXYZ[0]);
+    if(L_radian < 0){
+        L_radian += pi;
+    }
+    double L = rad2degree(L_radian);
+    double sqrtXY = sqrt(ptXYZ[0] * ptXYZ[0] + ptXYZ[1] * ptXYZ[1]);
+    double t0 = ptXYZ[2] / sqrtXY;
+    double p = c * e2 / sqrtXY;
+    double k = 1 + ep2;
+    double ti = t0;
+    double ti1 = 0.0;
+    int loop = 0;
+    while(loop<10000){
+        loop += 1;
+        ti1 = t0 + p * ti / sqrt(k + ti * ti);
+        if(abs(ti1 - ti) < 1e-12)
+            break;
+        ti=ti1;
+    }
+    double B_radian = atan(ti1);
+    double N = GetN(B_radian);
+    double H = sqrtXY / cos(B_radian) - N;
+    double B = rad2degree(B_radian);
+    return Array({B,L,H});
+}
+matrix GetR_ENU2ECEF(const double& radianL,const double& radianB){
+    double sinB = sin(radianB);
+    double cosB = cos(radianB);
+    double sinL = sin(radianL);
+    double cosL = cos(radianL);
+    matrix R4 = matrix({{-sinL, -sinB * cosL, cosB * cosL},
+			{cosL, -sinB * sinL, cosB * sinL},
+			{0, cosB, sinB}});
+    return R4;
+}
+double GetAngleDif(double distance){
+    double angleRafian = distance / a;
+    return rad2degree(angleRafian);
+}
+Array GetAngleDif(Array& distance){
+    Array angleRafian = distance / a;
+    return rad2degree(angleRafian);
+}
+
+} //namespace jfx
\ No newline at end of file

coordinate/wgs84.h

+#ifndef JFX_WGS84_H
+#define JFX_WGS84_H
+
+#include "matrix.h"
+#include "rotations.h"
+#include <map>
+#include <math.h>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace jfx {
+
+const double a = 6378137.0;
+const double f = 0.003352810664;
+const double b = 6356752.3142499477;
+const double c = 6399593.6257536924;
+const double e2 = 0.006694379988651241;
+const double ep2 = 0.0067394967407662064;
+
+const double m0 = 6335439.3273023246;
+const double m2 = 63617.757378010137;
+const double m4 = 532.35180239277622;
+const double m6 = 4.1577261283373916;
+const double m8 = 0.031312573415813519;
+
+const double a0 = 6367449.1457686741;
+const double a2 = 32077.017223574985;
+const double a4 = 67.330398573595;
+const double a6 = 0.13188597734903185;
+const double a8 = 0.00024462947981104312;
+
+
+double GetN(const double& radB);
+double GetX(const double& radB);
+double GetLatByX(const double& X);
+double GetV(const double& lat);
+Array BLH2ECEF(const Array& ptBLH);
+Array ECEF2BLH(const Array& ptXYZ);
+matrix GetR_ENU2ECEF(const double& radianL,const double& radianB);
+double GetAngleDif(double distance);
+Array GetAngleDif(Array& distance);
+
+} // namespace jfx
+
+#endif // __JFX_WGS84_H__
\ No newline at end of file