modularization/src/detection/vision_TensorRT_yolov3_Track/localization.h

#ifndef __LOCALIZATION_H__
#define __LOCALIZATION_H__


#include <iostream>
#include <cassert>
#include <eigen3/Eigen/Dense>

using namespace std;

//用于标定传感器相当于标准坐标系的正向位置
//当数据为正（+）或者无符号时，表示当投影到标准坐标系中时，位于投影坐标轴的正向空间
//当数据为负（-），表示当投影到标准坐标系中时，位于投影坐标轴的负向空间

//用于标定毫米波雷达（Radar）相对于标准坐标系的正向位置
#define X_Radar 0
#define Y_Radar 465

//用于标定激光雷达（Lidar）相对于标准坐标系的正向位置
#define X_Lidar 0
#define Y_Lidar 0

//用于标定GPS模块相当于标准坐标系的正向位置
#define X_GPS 0
#define Y_GPS 0

//用于标定相机（camera）相当于标准坐标系的正向位置
#define X_Camera 0
#define Y_Camera 188
#define Z_Camera 0

using Eigen::MatrixXd;

class TranslateToStandardLocation
{
public:
    static inline MatrixXd translateRadarToStandardLocation(MatrixXd radarLoc)
    {
        MatrixXd staLoc = radarLoc;
        staLoc(1, 0) = staLoc(1, 0) - X_Radar;
        staLoc(0, 1) = staLoc(0, 1) - Y_Radar;

        return staLoc;
    }

    static inline MatrixXd translateLidarToStandardLocation(MatrixXd lidarLoc)
    {
        MatrixXd staLoc = lidarLoc;
        staLoc(1, 0) = staLoc(1, 0) - X_Lidar;
        staLoc(0, 1) = staLoc(0, 1) - Y_Lidar;

        return staLoc;
    }

    static inline MatrixXd translateGPSToStandardLocation(MatrixXd GPSLoc)
    {
        MatrixXd staLoc = GPSLoc;
        staLoc(1, 0) = staLoc(1, 0) - X_GPS;
        staLoc(0, 1) = staLoc(0, 1) - Y_GPS;

        return staLoc;
    }

    static inline MatrixXd translateCameraToStandardLocation(MatrixXd cameraLoc)
    {
        assert(cameraLoc.rows() == 2);
        assert(cameraLoc.cols() == 1);

        MatrixXd staLoc(3, 1);
        staLoc(0, 0) = cameraLoc(0, 0);
        staLoc(1, 0) = cameraLoc(1, 0);
        staLoc(2, 0) = 1;

//        std::cout << staLoc << std::endl << std::endl;

        MatrixXd Mat(3, 3);
        Mat << 
               -4.5288434998620575e-01, -2.7615788020977561e-01,7.9366326248964913e+02,
               -9.4286502898149827e-02,-1.7340325696936468e-01, 1.8919238200384973e+02,
               -1.2880669794829137e-04, -3.2569693624068995e-04, 1.;
//            -1.82255446e-01, +1.92433308e-03, +1.92772928e+02,
//            -3.54541567e-02, -1.35423705e-01, -1.77699852e+01,
//            -1.53689475e-04, -1.33359828e-03, +1.00000000e+00;

//       std::cout << Mat << std::endl;

        //std::cout<< staLoc * Mat << endl;
        MatrixXd T(2, 1);
        T(0, 0) = X_Camera;
        T(1, 0) = Y_Camera;
        //T(2, 0) = Z_Camera;

        //MatrixXd m2 = Mat * staLoc + T;
        MatrixXd m2 = Mat * staLoc;
//        std::cout << m2 << std::endl
//                  << std::endl;
        assert(m2(2, 0) != 0);

        MatrixXd retMat(2, 1);
        retMat(0, 0) = m2(0, 0) / m2(2, 0);
        retMat(1, 0) = m2(1, 0) / m2(2, 0);

//        std::cout << (retMat + T) << std::endl<<std::endl;

        return retMat + T;
    }
};


#endif //__LOCALIZATION_H__