fusion.hpp 14 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310
  1. #ifndef FUSION_HPP
  2. #define FUSION_HPP
  3. #include "fusion_probabilities.h"
  4. #include "fusionobjectarray.pb.h"
  5. #include <iostream>
  6. #include "opencv2/opencv.hpp"
  7. #include "objectarray.pb.h"
  8. #include "mobileye.pb.h"
  9. #include "Eigen/Eigen"
  10. namespace iv {
  11. namespace fusion {
  12. //std::vector<Match_index> match_idxs;
  13. static float time_step = 0.3;
  14. static std::vector<std::string> labels = {"unknown","ped","bike","car","bus_or_truck"};
  15. float ComputeDis(cv::Point2f po1, cv::Point2f po2)
  16. {
  17. return (sqrt(pow((po1.x-po2.x),2) + pow((po1.y-po2.y),2)));
  18. }
  19. void Get_AssociationMat(iv::lidar::objectarray& lidar_object_arry,iv::radar::radarobjectarray& radar_object_array,
  20. std::vector<match_index>& match_idx,std::vector<int>&radar_idx)
  21. {
  22. // std::cout<<" get mat begin "<<std::endl;
  23. int nlidar = lidar_object_arry.obj_size();
  24. int nradar = radar_object_array.obj_size();
  25. match_idx.clear();
  26. radar_idx.clear();
  27. for(int i = 0; i<nlidar; i++)
  28. {
  29. match_index match;
  30. match.nlidar = i;
  31. std::vector<int> fuindex;
  32. for(int j =0; j<nradar; j++)
  33. {
  34. if(radar_object_array.obj(j).bvalid() == false) continue;
  35. if(iv::fusion::FusionProbabilities::ComputRadarLidarmatch(radar_object_array.obj(j),lidar_object_arry.obj(i)))
  36. {
  37. fuindex.push_back(j);
  38. }
  39. }
  40. if(fuindex.size() >0)
  41. {
  42. std::vector<float> dis;
  43. cv::Point2f po1;
  44. po1.x = lidar_object_arry.obj(i).centroid().x();
  45. po1.y = lidar_object_arry.obj(i).centroid().y();
  46. for(int index =0; index< fuindex.size(); index++)
  47. {
  48. cv::Point2f po2;
  49. po2.x = radar_object_array.obj(fuindex[index]).x();
  50. po2.y = radar_object_array.obj(fuindex[index]).y();
  51. dis.push_back(ComputeDis(po1,po2));
  52. }
  53. // std::cout<<" x y : "<<po1.x<<" "<<po1.y<<std::endl;
  54. auto smallest = std::min_element(std::begin(dis), std::end(dis));
  55. int index_ = std::distance(std::begin(dis), smallest);
  56. dis.clear();
  57. match.nradar = fuindex[index_];
  58. }else {
  59. match.nradar = -1000;
  60. }
  61. match_idx.push_back(match);
  62. }
  63. //std::cout<<" match_size : "<<match_idx.size()<<" "<<match_idx[0].nradar<<std::endl;
  64. for(int k = 0; k<radar_object_array.obj_size(); k++)
  65. {
  66. std::vector<int> indexs;
  67. for(int radar_idx =0; radar_idx<match_idx.size(); radar_idx++)
  68. {
  69. indexs.push_back(match_idx[radar_idx].nradar);
  70. }
  71. if(radar_object_array.obj(k).bvalid() == false) continue;
  72. if(abs(radar_object_array.obj(k).x())< 2 && radar_object_array.obj(k).y()< 100 ){
  73. if(!(std::find(indexs.begin(),indexs.end(),k) !=indexs.end()))
  74. {
  75. radar_idx.push_back(k);
  76. // std::cout<<" x y "<<radar_object_array.obj(k).x()<<" "<<radar_object_array.obj(k).y()<<std::endl;
  77. }
  78. }}
  79. // std::cout<<" radar_size : "<<" "<<radar_idx.size()<<std::endl;
  80. // std::cout<<" get mat end "<<std::endl;
  81. }
  82. void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarray& radar_object_array, iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
  83. {
  84. // std::cout<< " get fusion begin "<<std::endl;
  85. lidar_radar_fusion_object_array.Clear();
  86. std::vector<match_index> match_idx;
  87. std::vector<int> radar_idx;
  88. Get_AssociationMat(lidar_object_arr,radar_object_array,match_idx,radar_idx);
  89. for(int i =0; i< match_idx.size(); i++)
  90. {
  91. iv::fusion::fusionobject fusion_object;
  92. fusion_object.set_id(lidar_object_arr.obj(match_idx[i].nlidar).id());
  93. fusion_object.set_sensor_type(0);
  94. fusion_object.set_type(lidar_object_arr.obj(match_idx[i].nlidar).mntype());
  95. fusion_object.set_prob(lidar_object_arr.obj(match_idx[i].nlidar).score());
  96. if(match_idx.at(i).nradar == -1000)
  97. {
  98. // std::cout<<" fusion is ok "<<std::endl;
  99. if( lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x() *
  100. lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y() > 18)
  101. {
  102. fusion_object.set_yaw(1.57);
  103. } else {
  104. fusion_object.set_yaw(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
  105. }
  106. iv::fusion::VelXY vel_relative;
  107. iv::fusion::VelXY *vel_relative_;
  108. vel_relative.set_x(lidar_object_arr.obj(match_idx[i].nlidar).vel_relative().x());
  109. vel_relative.set_y(lidar_object_arr.obj(match_idx[i].nlidar).vel_relative().y());
  110. vel_relative_ = fusion_object.mutable_vel_relative();
  111. vel_relative_->CopyFrom(vel_relative);
  112. iv::fusion::PointXYZ centroid;
  113. iv::fusion::PointXYZ *centroid_;
  114. centroid.set_x(lidar_object_arr.obj(match_idx[i].nlidar).centroid().x());
  115. centroid.set_y(lidar_object_arr.obj(match_idx[i].nlidar).centroid().y());
  116. centroid.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z());
  117. centroid_ = fusion_object.mutable_centroid();
  118. centroid_->CopyFrom(centroid);
  119. std::cout<<"lidar: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
  120. }else {
  121. // std::cout<<" fusion is ok "<<std::endl;
  122. fusion_object.set_yaw(radar_object_array.obj(match_idx.at(i).nradar).angle());
  123. fusion_object.set_lifetime(radar_object_array.obj(match_idx.at(i).nradar).has_bridge_object());
  124. iv::fusion::VelXY vel_relative;
  125. iv::fusion::VelXY *vel_relative_;
  126. // vel_relative.set_x(radar_object_array.obj(match_idx.at(i).nradar).vx());
  127. // vel_relative.set_y(radar_object_array.obj(match_idx.at(i).nradar).vy());
  128. vel_relative.set_x(lidar_object_arr.obj(match_idx[i].nlidar).vel_relative().x());
  129. vel_relative.set_y(lidar_object_arr.obj(match_idx[i].nlidar).vel_relative().y());
  130. vel_relative_ = fusion_object.mutable_vel_relative();
  131. vel_relative_->CopyFrom(vel_relative);
  132. iv::fusion::VelXY vel_abs;
  133. iv::fusion::VelXY *vel_abs_;
  134. vel_abs.set_x(radar_object_array.obj(match_idx.at(i).nradar).vx());
  135. vel_abs.set_y(radar_object_array.obj(match_idx.at(i).nradar).vy());
  136. vel_abs_ = fusion_object.mutable_vel_abs();
  137. vel_abs_->CopyFrom(vel_abs);
  138. iv::fusion::PointXYZ centroid;
  139. iv::fusion::PointXYZ *centroid_;
  140. centroid.set_x(radar_object_array.obj(match_idx[i].nradar).x());
  141. centroid.set_y(radar_object_array.obj(match_idx[i].nradar).y());
  142. centroid.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z());
  143. centroid_ = fusion_object.mutable_centroid();
  144. centroid_->CopyFrom(centroid);
  145. #ifdef DEBUG_SHOW
  146. std::cout<<"radar: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
  147. #endif
  148. }
  149. iv::fusion::Dimension dimension;
  150. iv::fusion::Dimension *dimension_;
  151. dimension.set_x(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x());
  152. dimension.set_y(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y());
  153. dimension.set_z(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().z());
  154. dimension_ = fusion_object.mutable_dimensions();
  155. dimension_->CopyFrom(dimension);
  156. std::cout<<" x y z: "<<lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()<<" "<<lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()<<std::endl;
  157. iv::fusion::fusionobject *pobj = lidar_radar_fusion_object_array.add_obj();
  158. pobj->CopyFrom(fusion_object);
  159. }
  160. for(int j = 0; j< radar_idx.size() ; j++){
  161. break;
  162. iv::fusion::fusionobject fusion_obj;
  163. fusion_obj.set_yaw(radar_object_array.obj(radar_idx[j]).angle());
  164. iv::fusion::VelXY vel_relative;
  165. iv::fusion::VelXY *vel_relative_;
  166. vel_relative.set_x(radar_object_array.obj(radar_idx[j]).vx());
  167. vel_relative.set_y(radar_object_array.obj(radar_idx[j]).vy());
  168. vel_relative_ = fusion_obj.mutable_vel_relative();
  169. vel_relative_->CopyFrom(vel_relative);
  170. iv::fusion::PointXYZ centroid;
  171. iv::fusion::PointXYZ *centroid_;
  172. centroid.set_x(radar_object_array.obj(radar_idx[j]).x());
  173. centroid.set_y(radar_object_array.obj(radar_idx[j]).y());
  174. centroid.set_z(1.0);
  175. centroid_ = fusion_obj.mutable_centroid();
  176. centroid_->CopyFrom(centroid);
  177. iv::fusion::Dimension dimension;
  178. iv::fusion::Dimension *dimension_;
  179. dimension.set_x(0.5);
  180. dimension.set_y(0.5);
  181. dimension.set_z(1.0);
  182. dimension_ = fusion_obj.mutable_dimensions();
  183. dimension_->CopyFrom(dimension);
  184. iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
  185. obj->CopyFrom(fusion_obj);
  186. }
  187. // std::cout<<" fusion end "<<std::endl;
  188. }
  189. void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array, iv::mobileye::mobileye& xobs_info)
  190. {
  191. int time_step = 0.3;
  192. // lidar_radar_fusion_object_array.Clear();
  193. for(int j = 0; j< xobs_info.xobj_size() ; j++){
  194. iv::fusion::fusionobject fusion_obj;
  195. fusion_obj.set_sensor_type(1);
  196. fusion_obj.set_yaw(0);
  197. iv::fusion::VelXY vel_relative;
  198. iv::fusion::VelXY *vel_relative_;
  199. if(xobs_info.xobj(j).obstype() == iv::mobileye::obs_OBSTYPE::obs_OBSTYPE_PED)
  200. {
  201. fusion_obj.set_type(2);
  202. }else{
  203. fusion_obj.set_type(1);
  204. vel_relative.set_x(xobs_info.xobj(j).obs_rel_vel_x() * tan(xobs_info.xobj(j).obsang()));
  205. vel_relative.set_y(xobs_info.xobj(j).obs_rel_vel_x());
  206. }
  207. vel_relative_ = fusion_obj.mutable_vel_relative();
  208. vel_relative_->CopyFrom(vel_relative);
  209. iv::fusion::PointXYZ centroid;
  210. iv::fusion::PointXYZ *centroid_;
  211. centroid.set_x(-(xobs_info.xobj(j).pos_y()));
  212. centroid.set_y(xobs_info.xobj(j).pos_x());
  213. centroid.set_z(1.0);
  214. centroid_ = fusion_obj.mutable_centroid();
  215. centroid_->CopyFrom(centroid);
  216. #ifdef DEBUG_SHOW
  217. std::cout<<"mobileye: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
  218. #endif
  219. iv::fusion::Dimension dimension;
  220. iv::fusion::Dimension *dimension_;
  221. dimension.set_x(xobs_info.xobj(j).obswidth());
  222. dimension.set_y(2.0);
  223. dimension.set_z(1.0);
  224. dimension_ = fusion_obj.mutable_dimensions();
  225. dimension_->CopyFrom(dimension);
  226. iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
  227. obj->CopyFrom(fusion_obj);
  228. }
  229. }
  230. void ObsToNormal(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
  231. {
  232. for(int i = 0; i < lidar_radar_fusion_object_array.obj_size(); i++)
  233. {
  234. 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;
  235. 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;
  236. 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;
  237. if((lidar_radar_fusion_object_array.obj(i).dimensions().x()>0)&&
  238. (lidar_radar_fusion_object_array.obj(i).dimensions().y()>0))
  239. {
  240. int xp = (int)((lidar_radar_fusion_object_array.obj(i).dimensions().x()/0.2)/2.0);
  241. if(xp == 0)xp=1;
  242. int yp = (int)((lidar_radar_fusion_object_array.obj(i).dimensions().y()/0.2)/2.0);
  243. if(yp == 0)yp=1;
  244. int ix,iy;
  245. for(ix = 0; ix<(xp*2); ix++)
  246. {
  247. for(iy = 0; iy<(yp*2); iy++)
  248. {
  249. iv::fusion::NomalXYZ nomal_centroid;
  250. iv::fusion::NomalXYZ *nomal_centroid_;
  251. float nomal_x = ix*0.2 - xp*0.2;
  252. float nomal_y = iy*0.2 - yp*0.2;
  253. float nomal_z = 1.0;
  254. float s = nomal_x*cos(lidar_radar_fusion_object_array.obj(i).yaw())
  255. - nomal_y*sin(lidar_radar_fusion_object_array.obj(i).yaw());
  256. float t = nomal_x*sin(lidar_radar_fusion_object_array.obj(i).yaw())
  257. + nomal_y*cos(lidar_radar_fusion_object_array.obj(i).yaw());
  258. nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s);
  259. nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t);
  260. if(abs(lidar_radar_fusion_object_array.obj(i).centroid().x() + s) <1.3 &&
  261. lidar_radar_fusion_object_array.obj(i).centroid().y() + t <1.0) continue;
  262. else{
  263. nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s);
  264. nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t);
  265. iv::fusion::fusionobject &fusion_obj = (iv::fusion::fusionobject &)lidar_radar_fusion_object_array.obj(i);
  266. nomal_centroid_ = fusion_obj.add_nomal_centroid();
  267. nomal_centroid_->CopyFrom(nomal_centroid);
  268. }
  269. }
  270. }
  271. }
  272. }}
  273. }
  274. }
  275. #endif // FUSION_HPP