123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409 |
- #ifndef FUSION_HPP
- #define FUSION_HPP
- #include "fusion_probabilities.h"
- #include "fusionobjectarray.pb.h"
- #include <iostream>
- #include "opencv2/opencv.hpp"
- #include "objectarray.pb.h"
- #include "mobileye.pb.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"};
- 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_index>& match_idx,std::vector<int>&radar_idx)
- {
- // std::cout<<" get mat begin "<<std::endl;
- int nlidar = lidar_object_arry.obj_size();
- int nradar = radar_object_array.obj_size();
- match_idx.clear();
- radar_idx.clear();
- for(int i = 0; i<nlidar; i++)
- {
- match_index match;
- match.nlidar = i;
- std::vector<int> fuindex;
- for(int j =0; j<nradar; j++)
- {
- if(radar_object_array.obj(j).bvalid() == false) continue;
- if(iv::fusion::FusionProbabilities::ComputRadarLidarmatch(radar_object_array.obj(j),lidar_object_arry.obj(i)))
- {
- fuindex.push_back(j);
- }
- }
- if(fuindex.size() >0)
- {
- std::vector<float> 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< dis.size(); index++)
- {
- cv::Point2f po2;
- po2.x = radar_object_array.obj(dis[index]).x();
- po2.y = radar_object_array.obj(dis[index]).y();
- dis.push_back(ComputeDis(po1,po2));
- }
- // std::cout<<" x y : "<<po1.x<<" "<<po1.y<<std::endl;
- 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;
- }
- match_idx.push_back(match);
- }
- //std::cout<<" match_size : "<<match_idx.size()<<" "<<match_idx[0].nradar<<std::endl;
- for(int k = 0; k<radar_object_array.obj_size(); k++)
- {
- std::vector<int> indexs;
- for(int radar_idx =0; radar_idx<match_idx.size(); radar_idx++)
- {
- indexs.push_back(match_idx[radar_idx].nradar);
- }
- if(radar_object_array.obj(k).bvalid() == false) continue;
- if(abs(radar_object_array.obj(k).x())< 2 && radar_object_array.obj(k).y()< 100 ){
- if(!(std::find(indexs.begin(),indexs.end(),k) !=indexs.end()))
- {
- radar_idx.push_back(k);
- // std::cout<<" x y "<<radar_object_array.obj(k).x()<<" "<<radar_object_array.obj(k).y()<<std::endl;
- }
- }}
- // std::cout<<" radar_size : "<<" "<<radar_idx.size()<<std::endl;
- // std::cout<<" get mat end "<<std::endl;
- }
- void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarray& radar_object_array, iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
- {
- // std::cout<< " get fusion begin "<<std::endl;
- lidar_radar_fusion_object_array.Clear();
- std::vector<match_index> match_idx;
- std::vector<int> 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_type_name(lidar_object_arr.obj(match_idx[i].nlidar).type_name());
- 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 "<<std::endl;
- 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).position().x());
- vel_relative.set_y(lidar_object_arr.obj(match_idx[i].nlidar).position().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);
- }else {
- // std::cout<<" fusion is 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(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);
- }
- iv::fusion::Dimension dimension;
- iv::fusion::Dimension *dimension_;
- dimension.set_x(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x());
- dimension.set_y(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y());
- dimension.set_z(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().z());
- dimension_ = fusion_object.mutable_dimensions();
- dimension_->CopyFrom(dimension);
- if((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()>0)&&
- (lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()>0))
- {
- int xp = (int)((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()/0.2)/2.0);
- if(xp == 0)xp=1;
- int yp = (int)((lidar_object_arr.obj(match_idx[i].nlidar).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 = lidar_object_arr.obj(match_idx[i].nlidar).centroid().z();
- float s = nomal_x*cos(0) -
- nomal_y*sin(0);
- float t = nomal_x*sin(0) +
- nomal_y*cos(0);
- nomal_centroid.set_nomal_x(lidar_object_arr.obj(match_idx[i].nlidar).centroid().x() + s);
- nomal_centroid.set_nomal_y(lidar_object_arr.obj(match_idx[i].nlidar).centroid().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_arr.obj(match_idx[i].nlidar).centroid().x() +
- lidar_object_arr.obj(match_idx[i].nlidar).velocity_linear_x()*time_step*(k+1);
- float forcast_y = lidar_object_arr.obj(match_idx[i].nlidar).centroid().y() +
- lidar_object_arr.obj(match_idx[i].nlidar).velocity_linear_x()*time_step*(k+1);
- point_forcaste.set_x(forcast_x);
- point_forcaste.set_y(forcast_y);
- point_forcaste.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().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_arr.obj(match_idx[i].nlidar).dimensions().x()>0)&&
- (lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()>0))
- {
- int xp = (int)((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()/0.2)/2.0);
- if(xp == 0)xp=1;
- int yp = (int)((lidar_object_arr.obj(match_idx[i].nlidar).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_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_arr.obj(match_idx[i].nlidar).centroid().z();
- float s = nomal_x*cos(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()) -
- nomal_y*sin(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
- float t = nomal_x*sin(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()) +
- nomal_y*cos(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
- 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++){
- 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);
- 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(radar_idx[j]).angle())
- - nomal_y*sin(radar_object_array.obj(radar_idx[j]).angle());
- float t = nomal_x*sin(radar_object_array.obj(radar_idx[j]).angle())
- + nomal_y*cos(radar_object_array.obj(radar_idx[j]).angle());
- nomal_centroid.set_nomal_x(radar_object_array.obj(radar_idx[j]).x() + s);
- nomal_centroid.set_nomal_y(radar_object_array.obj(radar_idx[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(radar_idx[j]).x()
- + radar_object_array.obj(radar_idx[j]).vx()*time_step*(k+1);
- float forcast_y = radar_object_array.obj(radar_idx[j]).y()
- + radar_object_array.obj(radar_idx[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(k);
- 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(radar_idx[j]).angle())
- - nomal_y*sin(radar_object_array.obj(radar_idx[j]).angle());
- float t = nomal_x*sin(radar_object_array.obj(radar_idx[j]).angle())
- + nomal_y*cos(radar_object_array.obj(radar_idx[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);
- }
- // std::cout<<" fusion end "<<std::endl;
- }
- void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array, iv::mobileye::mobileye& xobs_info)
- {
- int time_step = 0.3;
- lidar_radar_fusion_object_array.Clear();
- for(int j = 0; j< xobs_info.xobj_size() ; j++){
- iv::fusion::fusionobject fusion_obj;
- fusion_obj.set_yaw(xobs_info.xobj(j).obsang());
- iv::fusion::VelXY vel_relative;
- iv::fusion::VelXY *vel_relative_;
- vel_relative.set_x(xobs_info.xobj(j).obs_rel_vel_x() * tan(xobs_info.xobj(j).obsang()));
- vel_relative.set_y(xobs_info.xobj(j).obs_rel_vel_x());
- vel_relative_ = fusion_obj.mutable_vel_relative();
- vel_relative_->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);
- iv::fusion::Dimension dimension;
- iv::fusion::Dimension *dimension_;
- dimension.set_x(xobs_info.xobj(j).obswidth());
- dimension.set_y(2.0);
- dimension.set_z(1.0);
- dimension_ = fusion_obj.mutable_dimensions();
- dimension_->CopyFrom(dimension);
- int xp = (int)((xobs_info.xobj(j).obswidth()/0.2)/2.0);
- if(xp == 0)xp=1;
- int yp = (int)((2.0/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(0)
- - nomal_y*sin(0);
- float t = nomal_x*sin(0)
- + nomal_y*cos(0);
- nomal_centroid.set_nomal_x(-xobs_info.xobj(j).pos_y() + s);
- nomal_centroid.set_nomal_y(xobs_info.xobj(j).pos_x() + t);
- nomal_centroid_ = fusion_obj.add_nomal_centroid();
- nomal_centroid_->CopyFrom(nomal_centroid);
- }
- }
- iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
- obj->CopyFrom(fusion_obj);
- }
- }
- }
- }
- #endif // FUSION_HPP
|