#include "fusion_probabilities.h"

//毫米波雷达object点是否和激光雷达object的俯视box匹配
int iv::fusion::FusionProbabilities::ComputRadarLidarmatch(const  iv::radar::radarobject& radarPoint, const  iv::lidar::lidarobject& lidarobject)
{
    Eigen::Matrix<float,3,1> radar_in_radar, radar_in_lidar;
    radar_in_radar << radarPoint.x(), radarPoint.y(),-0.95;
    radar_in_lidar = iv::fusion::Transformation::RadarToLidar(radar_in_radar);
//    std::cout<<" x    y   "<<lidarobject.centroid().x()<<"    "<<lidarobject.centroid().y()<<std::endl;
    if(!((radarPoint.x()>=(lidarobject.centroid().x() - lidarobject.dimensions().x()/2.0 -2))&&
         (radarPoint.y()>= (lidarobject.centroid().y()-lidarobject.dimensions().y()/2.0 -2 ))
         &&(radarPoint.x()<=(lidarobject.centroid().x() + lidarobject.dimensions().x()/2.0 +2))&&
         (radarPoint.y()<=(lidarobject.centroid().y() + lidarobject.dimensions().y()/2.0 +2))))
    {
        return 0;
    } else {
        return 1;
    }
}