modularization/src/fusion/lidar_radar_fusion_cnn/transformation.cpp

#include "transformation.h"

using namespace Eigen;
using namespace std;

//计算毫米波目标距离
float iv::fusion::Transformation::ComputelonDistance(double x, double y)
{
    return (sqrt(pow(x,2) + pow(y,2)));
}

//计算毫米波目标速度
float iv::fusion::Transformation::ComputeRadarSpeed(double vx, double vy)
{
    return (sqrt(pow(vx,2) + pow(vy,2)));
}

//radar to lidar
Eigen::Matrix<float,3,1> iv::fusion::Transformation::RadarToLidar(Eigen::Matrix<float,3,1> radar_int_radar)
{
    Eigen::Matrix<float,3,3> radar_to_lidar_R;
    radar_to_lidar_R << -0.07164891, -0.99734797, -0.01278487,
                        0.99733994,  -0.07180872, 0.01251175,
                        -0.01339663, -0.0118544,  0.99983999;
    Eigen::Matrix<float,3,1> radar_in_lidar;
    Eigen::Matrix<float,3,1> radar_to_lidar_T;
    radar_to_lidar_T << -0.35455075, -0.84757324, 0.26293475;
    radar_in_lidar = radar_to_lidar_R*radar_int_radar + radar_to_lidar_T;
    return radar_in_lidar;
}

//lidar to radar
Eigen::Matrix<float,3,1> iv::fusion::Transformation::LidarToRadar(Eigen::Matrix<float,3,1> lidar_in_lidar)
{
    Eigen::Matrix<float,3,3> lidar_to_radar_R;
    Eigen::Matrix<float,3,1> lidar_in_radar;
    Eigen::Matrix<float,3,1> lidar_to_radar_T;
    lidar_in_lidar = lidar_to_radar_R*lidar_in_lidar + lidar_to_radar_T;
    return lidar_in_radar;
}



//毫米波数据去重
//将速度相近、目标距离相近、纵向坐标相近的目标状态置为false
void iv::fusion::Transformation::RadarPross(iv::radar::radarobjectarray& radar_object)
{
    for(int r = 0; r<(radar_object.obj_size()-1);r++)
    {
        if(radar_object.obj(r).bvalid() == false) continue;
        for (int s=(r+1); s<(radar_object.obj_size());s++)
        {
            if(radar_object.obj(s).bvalid() == false) continue;
            float lonDistance1 = ComputelonDistance(radar_object.obj(r).x(),radar_object.obj(r).y());
            float lonDistance2 = ComputelonDistance(radar_object.obj(s).x(),radar_object.obj(s).y());
            float RadarSpeed1 = ComputeRadarSpeed(radar_object.obj(r).vx(),radar_object.obj(r).vy());
            float RadarSpeeds = ComputeRadarSpeed(radar_object.obj(s).vx(), radar_object.obj(s).vy());

            if(
                   (fabs(lonDistance1- lonDistance2) <= 1.00) &&
                   (fabs(radar_object.obj(r).x() - radar_object.obj(s).x()) <=1.00)&&
                   (fabs(RadarSpeed1-RadarSpeeds)<=1.00)
             )
            {
                iv::radar::radarobject & radar_objct_new = (iv::radar::radarobject&)radar_object.obj(r);
                radar_objct_new.set_bvalid(false);
            }

        }

    }
}