#ifndef FUSION_HPP #define FUSION_HPP #include "fusion_probabilities.h" #include "fusionobjectarray.pb.h" #include #include "opencv2/opencv.hpp" #include "objectarray.pb.h" #include "mobileye.pb.h" #include "Eigen/Eigen" namespace iv { namespace fusion { //std::vector match_idxs; static float time_step = 0.3; static std::vector labels = {"unknown","ped","bike","car","bus_or_truck"}; 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(iv::lidar::objectarray& lidar_object_arry,iv::radar::radarobjectarray& radar_object_array, std::vector& match_idx,std::vector&radar_idx) { // std::cout<<" get mat begin "< fuindex; for(int j =0; j0) { std::vector dis; cv::Point2f po1; po1.x = lidar_object_arry.obj(i).centroid().x(); po1.y = lidar_object_arry.obj(i).centroid().y(); for(int index =0; index< fuindex.size(); index++) { cv::Point2f po2; po2.x = radar_object_array.obj(fuindex[index]).x(); po2.y = radar_object_array.obj(fuindex[index]).y(); dis.push_back(ComputeDis(po1,po2)); } // std::cout<<" x y : "< indexs; for(int radar_idx =0; radar_idx match_idx; std::vector radar_idx; Get_AssociationMat(lidar_object_arr,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_arr.obj(match_idx[i].nlidar).id()); fusion_object.set_sensor_type(0); fusion_object.set_type(lidar_object_arr.obj(match_idx[i].nlidar).mntype()); fusion_object.set_prob(lidar_object_arr.obj(match_idx[i].nlidar).score()); if(match_idx.at(i).nradar == -1000) { // std::cout<<" fusion is ok "< 18) { fusion_object.set_yaw(1.57); } else { fusion_object.set_yaw(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()); } iv::fusion::VelXY vel_relative; iv::fusion::VelXY *vel_relative_; vel_relative.set_x(lidar_object_arr.obj(match_idx[i].nlidar).vel_relative().x()); vel_relative.set_y(lidar_object_arr.obj(match_idx[i].nlidar).vel_relative().y()); vel_relative_ = fusion_object.mutable_vel_relative(); vel_relative_->CopyFrom(vel_relative); iv::fusion::PointXYZ centroid; iv::fusion::PointXYZ *centroid_; centroid.set_x(lidar_object_arr.obj(match_idx[i].nlidar).centroid().x()); centroid.set_y(lidar_object_arr.obj(match_idx[i].nlidar).centroid().y()); centroid.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z()); centroid_ = fusion_object.mutable_centroid(); centroid_->CopyFrom(centroid); std::cout<<"lidar: "<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(radar_object_array.obj(match_idx[i].nradar).x()); centroid.set_y(radar_object_array.obj(match_idx[i].nradar).y()); centroid.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z()); centroid_ = fusion_object.mutable_centroid(); centroid_->CopyFrom(centroid); #ifdef DEBUG_SHOW std::cout<<"radar: "<CopyFrom(dimension); std::cout<<" x y z: "<CopyFrom(fusion_object); } for(int j = 0; j< radar_idx.size() ; j++){ break; iv::fusion::fusionobject fusion_obj; fusion_obj.set_yaw(radar_object_array.obj(radar_idx[j]).angle()); iv::fusion::VelXY vel_relative; iv::fusion::VelXY *vel_relative_; vel_relative.set_x(radar_object_array.obj(radar_idx[j]).vx()); vel_relative.set_y(radar_object_array.obj(radar_idx[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(radar_idx[j]).x()); centroid.set_y(radar_object_array.obj(radar_idx[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); iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj(); obj->CopyFrom(fusion_obj); } // std::cout<<" fusion end "<CopyFrom(vel_relative); iv::fusion::PointXYZ centroid; iv::fusion::PointXYZ *centroid_; centroid.set_x(-(xobs_info.xobj(j).pos_y())); centroid.set_y(xobs_info.xobj(j).pos_x()); centroid.set_z(1.0); centroid_ = fusion_obj.mutable_centroid(); centroid_->CopyFrom(centroid); #ifdef DEBUG_SHOW std::cout<<"mobileye: "<CopyFrom(dimension); iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj(); obj->CopyFrom(fusion_obj); } } void ObsToNormal(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array) { for(int i = 0; i < lidar_radar_fusion_object_array.obj_size(); i++) { if((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0) &&(lidar_radar_fusion_object_array.obj(i).centroid().y() > 20 || abs(lidar_radar_fusion_object_array.obj(i).centroid().x())>10) ) continue; if ((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0)&&(lidar_radar_fusion_object_array.obj(i).centroid().y()>10 && abs(lidar_radar_fusion_object_array.obj(i).centroid().x())<1.3)) continue; if((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0)&&(lidar_radar_fusion_object_array.obj(i).centroid().y() <1.0 && abs(lidar_radar_fusion_object_array.obj(i).centroid().x())<1.3)) continue; if((lidar_radar_fusion_object_array.obj(i).dimensions().x()>0)&& (lidar_radar_fusion_object_array.obj(i).dimensions().y()>0)) { int xp = (int)((lidar_radar_fusion_object_array.obj(i).dimensions().x()/0.2)/2.0); if(xp == 0)xp=1; int yp = (int)((lidar_radar_fusion_object_array.obj(i).dimensions().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 = 1.0; float s = nomal_x*cos(lidar_radar_fusion_object_array.obj(i).yaw()) - nomal_y*sin(lidar_radar_fusion_object_array.obj(i).yaw()); float t = nomal_x*sin(lidar_radar_fusion_object_array.obj(i).yaw()) + nomal_y*cos(lidar_radar_fusion_object_array.obj(i).yaw()); nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s); nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t); if(abs(lidar_radar_fusion_object_array.obj(i).centroid().x() + s) <1.3 && lidar_radar_fusion_object_array.obj(i).centroid().y() + t <1.0) continue; else{ nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s); nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t); iv::fusion::fusionobject &fusion_obj = (iv::fusion::fusionobject &)lidar_radar_fusion_object_array.obj(i); nomal_centroid_ = fusion_obj.add_nomal_centroid(); nomal_centroid_->CopyFrom(nomal_centroid); } } } } }} } } #endif // FUSION_HPP