modularization/src/fusion/lidar_radar_fusion/fusion.hpp

#ifndef FUSION_HPP
#define FUSION_HPP
#include "fusion_probabilities.h"
#include "fusionobjectarray.pb.h"
#include <iostream>
#include "opencv2/opencv.hpp"
#include "perceptionoutput.h"
#include "Eigen/Eigen"

namespace iv {
namespace fusion {

//std::vector<Match_index> match_idxs;

static float time_step = 0.3;
static std::vector<std::string> labels = {"unknown","ped","bike","car","bus_or_truck"};

typedef  std::vector<iv::Perception::PerceptionOutput> LidarObject;
float ComputeDis(cv::Point2f po1, cv::Point2f po2)
{
    return (sqrt(pow((po1.x-po2.x),2) + pow((po1.y-po2.y),2)));
}

void Get_AssociationMat(LidarObject& lidar_object_vector,iv::radar::radarobjectarray& radar_object_array,
                        std::vector<match_index>& match_idx,std::vector<int>&radar_idx)
{
//    std::cout<<" is      ok     "<<std::endl;
    int nlidar = lidar_object_vector.size();
    int nradar = radar_object_array.obj_size();
    match_idx.clear();
    radar_idx.clear();
    for(int i = 0; i<lidar_object_vector.size(); i++)
    {
        match_index match;
        match.nlidar = i;
        std::vector<int> fuindex;
//        std::cout<<"  size    "<<radar_object_array.obj_size()<<std::endl;
        for(int j =0; j<radar_object_array.obj_size(); j++)
        {
//            std::cout<<"  is     ok   "<<std::endl;
            if(radar_object_array.obj(j).bvalid() == false) continue;
            if(!(iv::fusion::FusionProbabilities::ComputRadarLidarmatch(radar_object_array.obj(j),lidar_object_vector.at(i))))
            {
                fuindex.push_back(j);
            }
        }
        if(fuindex.size() >0)
        {
            std::vector<float> dis;
            cv::Point2f po1;
            po1.x = lidar_object_vector.at(i).location.x;
            po1.y = lidar_object_vector.at(i).location.y;
            for(std::vector<int>::iterator d = fuindex.begin(); d !=fuindex.end(); d++)
            {
                cv::Point2f po2;
                po2.x = radar_object_array.obj(*d).x();
                po2.y = radar_object_array.obj(*d).y();
                dis.push_back(ComputeDis(po1,po2));
            }
            auto smallest = std::min_element(std::begin(dis), std::end(dis));
            int index = std::distance(std::begin(dis), smallest);
            dis.clear();
            match.nradar = index;
        }else {
            match.nradar = -1000;
        }
//        std::cout<<" fuindex     nradar  "<<fuindex.size()<<"    "<<match.nradar<<std::endl;

        match_idx.push_back(match);
    }

    for(int k = 0; k<radar_object_array.obj_size(); k++)
    {
        std::vector<int> index;
        for(std::vector<match_index>::iterator l = match_idx.begin(); l != match_idx.end(); l++)
        {
            index.push_back(l->nradar);
        }
        if(std::find(index.begin(),index.end(),k) !=index.end())
        {
            radar_idx.push_back(k);
        }
    }
}

void RLfusion(LidarObject& lidar_object_vector,iv::radar::radarobjectarray& radar_object_array, iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
{
    lidar_radar_fusion_object_array.Clear();
    std::vector<match_index> match_idx;
    std::vector<int> radar_idx;
    Get_AssociationMat(lidar_object_vector,radar_object_array,match_idx,radar_idx);
    for(int i =0; i< match_idx.size(); i++)
    {
        iv::fusion::fusionobject fusion_object;
        fusion_object.set_id(lidar_object_vector.at(match_idx.at(i).nlidar).tracker_id);
        fusion_object.set_type_name(labels[lidar_object_vector.at(match_idx.at(i).nlidar).label]);
        fusion_object.set_prob(lidar_object_vector.at(match_idx.at(i).nlidar).robustness);
        if(match_idx.at(i).nradar == -1000)
        {
//            std::cout<<"  lidar    ok  "<<std::endl;
            fusion_object.set_yaw(lidar_object_vector.at(match_idx.at(i).nlidar).yaw);
            fusion_object.set_lifetime(lidar_object_vector.at(match_idx.at(i).nlidar).visible_life);

            iv::fusion::VelXY vel_relative;
            iv::fusion::VelXY *vel_relative_;
            vel_relative.set_x(lidar_object_vector.at(match_idx.at(i).nlidar).velocity.x);
            vel_relative.set_y(lidar_object_vector.at(match_idx.at(i).nlidar).velocity.y);
            vel_relative_ = fusion_object.mutable_vel_relative();
            vel_relative_->CopyFrom(vel_relative);

            iv::fusion::VelXY vel_abs;
            iv::fusion::VelXY *vel_abs_;
            vel_abs.set_x(lidar_object_vector.at(match_idx.at(i).nlidar).velocity_abs.x);
            vel_abs.set_y(lidar_object_vector.at(match_idx.at(i).nlidar).velocity_abs.y);
            vel_abs_ = fusion_object.mutable_vel_abs();
            vel_abs_->CopyFrom(vel_abs);

            iv::fusion::PointXYZ centroid;
            iv::fusion::PointXYZ *centroid_;
            centroid.set_x(lidar_object_vector.at(match_idx.at(i).nlidar).location.x);
            centroid.set_y(lidar_object_vector.at(match_idx.at(i).nlidar).location.y);
            centroid.set_z(lidar_object_vector.at(match_idx.at(i).nlidar).location.z);
            centroid_ = fusion_object.mutable_centroid();
            centroid_->CopyFrom(centroid);
        }else {
//            std::cout<<"  radar                           ok  "<<std::endl;
            fusion_object.set_yaw(radar_object_array.obj(match_idx.at(i).nradar).angle());
            fusion_object.set_lifetime(radar_object_array.obj(match_idx.at(i).nradar).has_bridge_object());

            iv::fusion::VelXY vel_relative;
            iv::fusion::VelXY *vel_relative_;
            vel_relative.set_x(radar_object_array.obj(match_idx.at(i).nradar).vx());
            vel_relative.set_y(radar_object_array.obj(match_idx.at(i).nradar).vy());
            vel_relative_ = fusion_object.mutable_vel_relative();
            vel_relative_->CopyFrom(vel_relative);

            iv::fusion::VelXY vel_abs;
            iv::fusion::VelXY *vel_abs_;
            vel_abs.set_x(radar_object_array.obj(match_idx.at(i).nradar).vx());
            vel_abs.set_y(radar_object_array.obj(match_idx.at(i).nradar).vy());
            vel_abs_ = fusion_object.mutable_vel_abs();
            vel_abs_->CopyFrom(vel_abs);

            iv::fusion::PointXYZ centroid;
            iv::fusion::PointXYZ *centroid_;
            centroid.set_x(lidar_object_vector.at(match_idx.at(i).nradar).location.x);
            centroid.set_y(lidar_object_vector.at(match_idx.at(i).nradar).location.y);
            centroid.set_z(lidar_object_vector.at(match_idx.at(i).nradar).location.z);
            centroid_ = fusion_object.mutable_centroid();
            centroid_->CopyFrom(centroid);
        }
        iv::fusion::AccXY acc_relative;
        iv::fusion::AccXY *acc_relative_;
        acc_relative.set_x(lidar_object_vector.at(match_idx.at(i).nlidar).acceleration_abs.x);
        acc_relative.set_y(lidar_object_vector.at(match_idx.at(i).nlidar).acceleration_abs.y);
        acc_relative_ = fusion_object.mutable_acc_relative();
        acc_relative_->CopyFrom(acc_relative);

        iv::fusion::PointXYZ min_point;
        iv::fusion::PointXYZ *min_point_;
        min_point.set_x(lidar_object_vector.at(match_idx.at(i).nlidar).nearest_point.x);
        min_point.set_y(lidar_object_vector.at(match_idx.at(i).nlidar).nearest_point.y);
        min_point_ = fusion_object.mutable_min_point();
        min_point_->CopyFrom(min_point);

        iv::fusion::Dimension dimension;
        iv::fusion::Dimension *dimension_;
        dimension.set_x(lidar_object_vector.at(match_idx.at(i).nlidar).size.x);
        dimension.set_y(lidar_object_vector.at(match_idx.at(i).nlidar).size.y);
        dimension.set_z(lidar_object_vector.at(match_idx.at(i).nlidar).size.z);
        dimension_ = fusion_object.mutable_dimensions();
        dimension_->CopyFrom(dimension);

        if((lidar_object_vector.at(match_idx.at(i).nlidar).size.x>0)&&(lidar_object_vector.at(match_idx.at(i).nlidar).size.y>0))
        {
            int xp = (int)((lidar_object_vector.at(match_idx.at(i).nlidar).size.x/0.2)/2.0);
            if(xp == 0)xp=1;
            int yp = (int)((lidar_object_vector.at(match_idx.at(i).nlidar).size.y/0.2)/2.0);
            if(yp == 0)yp=1;
            int ix,iy;
            for(ix = 0; ix<(xp*2); ix++)
            {
                for(iy = 0; iy<(yp*2); iy++)
                {
                    iv::fusion::NomalXYZ nomal_centroid;
                    iv::fusion::NomalXYZ *nomal_centroid_;
                    float nomal_x = ix*0.2 - xp*0.2;
                    float nomal_y = iy*0.2 - yp*0.2;
                    float nomal_z = lidar_object_vector.at(match_idx.at(i).nlidar).location.z;
                    float s = nomal_x*cos(lidar_object_vector.at(match_idx.at(i).nlidar).yaw) - nomal_y*sin(lidar_object_vector.at(match_idx.at(i).nlidar).yaw);
                    float t = nomal_x*sin(lidar_object_vector.at(match_idx.at(i).nlidar).yaw) + nomal_y*cos(lidar_object_vector.at(i).yaw);
                    nomal_centroid.set_nomal_x(lidar_object_vector.at(match_idx.at(i).nlidar).location.x + s);
                    nomal_centroid.set_nomal_y(lidar_object_vector.at(match_idx.at(i).nlidar).location.y+ t);
                    nomal_centroid_ = fusion_object.add_nomal_centroid();
                    nomal_centroid_->CopyFrom(nomal_centroid);
                }
            }
        }
        for(int k = 0; k<10; k++)
        {
//            std::cout<<"fusion    begin"<<std::endl;
            iv::fusion::PointXYZ point_forcaste;
            iv::fusion::PointXYZ *point_forcaste_;
            float forcast_x = lidar_object_vector.at(match_idx.at(i).nlidar).location.x + lidar_object_vector.at(match_idx.at(i).nlidar).velocity.x*time_step*(k+1);
            float forcast_y = lidar_object_vector.at(match_idx.at(i).nlidar).location.y + lidar_object_vector.at(match_idx.at(i).nlidar).velocity.y*time_step*(k+1);
            point_forcaste.set_x(forcast_x);
            point_forcaste.set_y(forcast_y);
            point_forcaste.set_z(lidar_object_vector.at(match_idx.at(i).nlidar).location.z);
            point_forcaste_ = fusion_object.add_point_forecast();
            point_forcaste_->CopyFrom(point_forcaste);

            iv::fusion::NomalForecast forcast_normal;
            iv::fusion::NomalForecast *forcast_normal_;
            forcast_normal.set_forecast_index(i);
//            std::cout<<"fusion     end"<<std::endl;
            if((lidar_object_vector.at(i).size.x>0)&&(lidar_object_vector.at(match_idx.at(i).nlidar).size.y>0))
            {
                int xp = (int)((lidar_object_vector.at(match_idx.at(i).nlidar).size.x/0.2)/2.0);
                if(xp == 0)xp=1;
                int yp = (int)((lidar_object_vector.at(match_idx.at(i).nlidar).size.y/0.2)/2.0);
                if(yp == 0)yp=1;
                int ix,iy;
                for(ix = 0; ix<(xp*2); ix++)
                {
                    for(iy = 0; iy<(yp*2); iy++)
                    {
                        iv::fusion::NomalXYZ nomal_forcast;
                        iv::fusion::NomalXYZ *nomal_forcast_;
                        float nomal_x = ix*0.2 - xp*0.2;
                        float nomal_y = iy*0.2 - yp*0.2;
                        float nomal_z = lidar_object_vector.at(match_idx.at(i).nlidar).location.z;
                        float s = nomal_x*cos(lidar_object_vector.at(match_idx.at(i).nlidar).yaw) - nomal_y*sin(lidar_object_vector.at(match_idx.at(i).nlidar).yaw);
                        float t = nomal_x*sin(lidar_object_vector.at(match_idx.at(i).nlidar).yaw) + nomal_y*cos(lidar_object_vector.at(match_idx.at(i).nlidar).yaw);
                        nomal_forcast.set_nomal_x(forcast_x + s);
                        nomal_forcast.set_nomal_y(forcast_y + t);
                        nomal_forcast_ = forcast_normal.add_forecast_nomal();
                        nomal_forcast_->CopyFrom(nomal_forcast);
                    }
                }
            }
            forcast_normal_=fusion_object.add_forecast_nomals();
            forcast_normal_->CopyFrom(forcast_normal);
        }

        iv::fusion::fusionobject *pobj = lidar_radar_fusion_object_array.add_obj();
        pobj->CopyFrom(fusion_object);
    }
        for(int j = 0; j< radar_idx.size() ; j++){
            if(radar_object_array.obj(j).bvalid() == false) continue;
            if(abs(radar_object_array.obj(j).x())<4 && radar_object_array.obj(j).y()< 100) continue;

            iv::fusion::fusionobject fusion_obj;
            fusion_obj.set_yaw(radar_object_array.obj(j).angle());
            iv::fusion::VelXY vel_relative;
            iv::fusion::VelXY *vel_relative_;
            vel_relative.set_x(radar_object_array.obj(j).vx());
            vel_relative.set_y(radar_object_array.obj(j).vy());
            vel_relative_ = fusion_obj.mutable_vel_relative();
            vel_relative_->CopyFrom(vel_relative);

            iv::fusion::PointXYZ centroid;
            iv::fusion::PointXYZ *centroid_;
            centroid.set_x(radar_object_array.obj(j).x());
            centroid.set_y(radar_object_array.obj(j).y());
            centroid.set_z(1.0);
            centroid_ = fusion_obj.mutable_centroid();
            centroid_->CopyFrom(centroid);

            iv::fusion::Dimension dimension;
            iv::fusion::Dimension *dimension_;
            dimension.set_x(0.5);
            dimension.set_y(0.5);
            dimension.set_z(1.0);
            dimension_ = fusion_obj.mutable_dimensions();
            dimension_->CopyFrom(dimension);

            int xp = (int)((0.5/0.2)/2.0);
            if(xp == 0)xp=1;
            int yp = (int)((0.5/0.2)/2.0);
            if(yp == 0)yp=1;
            int ix,iy;
            for(ix = 0; ix<(xp*2); ix++)
            {
                for(iy = 0; iy<(yp*2); iy++)
                {
                    iv::fusion::NomalXYZ nomal_centroid;
                    iv::fusion::NomalXYZ *nomal_centroid_;
                    float nomal_x = ix*0.2 - xp*0.2;
                    float nomal_y = iy*0.2 - yp*0.2;
                    float nomal_z = 1.0;
                    float s = nomal_x*cos(radar_object_array.obj(j).angle()) - nomal_y*sin(radar_object_array.obj(j).angle());
                    float t = nomal_x*sin(radar_object_array.obj(j).angle()) + nomal_y*cos(radar_object_array.obj(j).angle());
                    nomal_centroid.set_nomal_x(radar_object_array.obj(j).x() + s);
                    nomal_centroid.set_nomal_y(radar_object_array.obj(j).y() + t);
                    nomal_centroid_ = fusion_obj.add_nomal_centroid();
                    nomal_centroid_->CopyFrom(nomal_centroid);
                }
            }

            for(int k = 0; k<10; k++)
            {
//                std::cout<<"fusion    begin"<<std::endl;
                iv::fusion::PointXYZ point_forcaste;
                iv::fusion::PointXYZ *point_forcaste_;
                float forcast_x = radar_object_array.obj(j).x() + radar_object_array.obj(j).vx()*time_step*(k+1);
                float forcast_y = radar_object_array.obj(j).y() + radar_object_array.obj(j).vy()*time_step*(k+1);
                point_forcaste.set_x(forcast_x);
                point_forcaste.set_y(forcast_y);
                point_forcaste.set_z(1.0);
                point_forcaste_ = fusion_obj.add_point_forecast();
                point_forcaste_->CopyFrom(point_forcaste);

                iv::fusion::NomalForecast forcast_normal;
                iv::fusion::NomalForecast *forcast_normal_;
                forcast_normal.set_forecast_index(j);

                int xp = (int)((0.5/0.2)/2.0);
                if(xp == 0)xp=1;
                int yp = (int)((0.5/0.2)/2.0);
                if(yp == 0)yp=1;
                int ix,iy;
                for(ix = 0; ix<(xp*2); ix++)
                {
                    for(iy = 0; iy<(yp*2); iy++)
                    {
                        iv::fusion::NomalXYZ nomal_forcast;
                        iv::fusion::NomalXYZ *nomal_forcast_;
                        float nomal_x = ix*0.2 - xp*0.2;
                        float nomal_y = iy*0.2 - yp*0.2;
                        float nomal_z = 1.0;
                        float s = nomal_x*cos(radar_object_array.obj(j).angle()) - nomal_y*sin(radar_object_array.obj(j).angle());
                        float t = nomal_x*sin(radar_object_array.obj(j).angle()) + nomal_y*cos(radar_object_array.obj(j).angle());
                        nomal_forcast.set_nomal_x(forcast_x + s);
                        nomal_forcast.set_nomal_y(forcast_y + t);
                        nomal_forcast_ = forcast_normal.add_forecast_nomal();
                        nomal_forcast_->CopyFrom(nomal_forcast);
                    }
                }
                forcast_normal_=fusion_obj.add_forecast_nomals();
                forcast_normal_->CopyFrom(forcast_normal);
            }

            iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
            obj->CopyFrom(fusion_obj);
        }  
}
}
}


#endif // FUSION_HPP