fusion.hpp 18 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409
  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< dis.size(); index++)
  47. {
  48. cv::Point2f po2;
  49. po2.x = radar_object_array.obj(dis[index]).x();
  50. po2.y = radar_object_array.obj(dis[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 = 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_type_name(lidar_object_arr.obj(match_idx[i].nlidar).type_name());
  94. fusion_object.set_prob(lidar_object_arr.obj(match_idx[i].nlidar).score());
  95. if(match_idx.at(i).nradar == -1000)
  96. {
  97. // std::cout<<" fusion is ok "<<std::endl;
  98. fusion_object.set_yaw(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
  99. iv::fusion::VelXY vel_relative;
  100. iv::fusion::VelXY *vel_relative_;
  101. vel_relative.set_x(lidar_object_arr.obj(match_idx[i].nlidar).position().x());
  102. vel_relative.set_y(lidar_object_arr.obj(match_idx[i].nlidar).position().y());
  103. vel_relative_ = fusion_object.mutable_vel_relative();
  104. vel_relative_->CopyFrom(vel_relative);
  105. iv::fusion::PointXYZ centroid;
  106. iv::fusion::PointXYZ *centroid_;
  107. centroid.set_x(lidar_object_arr.obj(match_idx[i].nlidar).centroid().x());
  108. centroid.set_y(lidar_object_arr.obj(match_idx[i].nlidar).centroid().y());
  109. centroid.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z());
  110. centroid_ = fusion_object.mutable_centroid();
  111. centroid_->CopyFrom(centroid);
  112. }else {
  113. // std::cout<<" fusion is ok "<<std::endl;
  114. fusion_object.set_yaw(radar_object_array.obj(match_idx.at(i).nradar).angle());
  115. fusion_object.set_lifetime(radar_object_array.obj(match_idx.at(i).nradar).has_bridge_object());
  116. iv::fusion::VelXY vel_relative;
  117. iv::fusion::VelXY *vel_relative_;
  118. vel_relative.set_x(radar_object_array.obj(match_idx.at(i).nradar).vx());
  119. vel_relative.set_y(radar_object_array.obj(match_idx.at(i).nradar).vy());
  120. vel_relative_ = fusion_object.mutable_vel_relative();
  121. vel_relative_->CopyFrom(vel_relative);
  122. iv::fusion::VelXY vel_abs;
  123. iv::fusion::VelXY *vel_abs_;
  124. vel_abs.set_x(radar_object_array.obj(match_idx.at(i).nradar).vx());
  125. vel_abs.set_y(radar_object_array.obj(match_idx.at(i).nradar).vy());
  126. vel_abs_ = fusion_object.mutable_vel_abs();
  127. vel_abs_->CopyFrom(vel_abs);
  128. iv::fusion::PointXYZ centroid;
  129. iv::fusion::PointXYZ *centroid_;
  130. centroid.set_x(radar_object_array.obj(match_idx[i].nradar).x());
  131. centroid.set_y(radar_object_array.obj(match_idx[i].nradar).y());
  132. centroid.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z());
  133. centroid_ = fusion_object.mutable_centroid();
  134. centroid_->CopyFrom(centroid);
  135. }
  136. iv::fusion::Dimension dimension;
  137. iv::fusion::Dimension *dimension_;
  138. dimension.set_x(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x());
  139. dimension.set_y(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y());
  140. dimension.set_z(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().z());
  141. dimension_ = fusion_object.mutable_dimensions();
  142. dimension_->CopyFrom(dimension);
  143. if((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()>0)&&
  144. (lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()>0))
  145. {
  146. int xp = (int)((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()/0.2)/2.0);
  147. if(xp == 0)xp=1;
  148. int yp = (int)((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()/0.2)/2.0);
  149. if(yp == 0)yp=1;
  150. int ix,iy;
  151. for(ix = 0; ix<(xp*2); ix++)
  152. {
  153. for(iy = 0; iy<(yp*2); iy++)
  154. {
  155. iv::fusion::NomalXYZ nomal_centroid;
  156. iv::fusion::NomalXYZ *nomal_centroid_;
  157. float nomal_x = ix*0.2 - xp*0.2;
  158. float nomal_y = iy*0.2 - yp*0.2;
  159. float nomal_z = lidar_object_arr.obj(match_idx[i].nlidar).centroid().z();
  160. float s = nomal_x*cos(0) -
  161. nomal_y*sin(0);
  162. float t = nomal_x*sin(0) +
  163. nomal_y*cos(0);
  164. nomal_centroid.set_nomal_x(lidar_object_arr.obj(match_idx[i].nlidar).centroid().x() + s);
  165. nomal_centroid.set_nomal_y(lidar_object_arr.obj(match_idx[i].nlidar).centroid().y() + t);
  166. nomal_centroid_ = fusion_object.add_nomal_centroid();
  167. nomal_centroid_->CopyFrom(nomal_centroid);
  168. }
  169. }
  170. }
  171. for(int k = 0; k<10; k++)
  172. {
  173. // std::cout<<"fusion begin"<<std::endl;
  174. iv::fusion::PointXYZ point_forcaste;
  175. iv::fusion::PointXYZ *point_forcaste_;
  176. float forcast_x = lidar_object_arr.obj(match_idx[i].nlidar).centroid().x() +
  177. lidar_object_arr.obj(match_idx[i].nlidar).velocity_linear_x()*time_step*(k+1);
  178. float forcast_y = lidar_object_arr.obj(match_idx[i].nlidar).centroid().y() +
  179. lidar_object_arr.obj(match_idx[i].nlidar).velocity_linear_x()*time_step*(k+1);
  180. point_forcaste.set_x(forcast_x);
  181. point_forcaste.set_y(forcast_y);
  182. point_forcaste.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z());
  183. point_forcaste_ = fusion_object.add_point_forecast();
  184. point_forcaste_->CopyFrom(point_forcaste);
  185. iv::fusion::NomalForecast forcast_normal;
  186. iv::fusion::NomalForecast *forcast_normal_;
  187. forcast_normal.set_forecast_index(i);
  188. // std::cout<<"fusion end"<<std::endl;
  189. if((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()>0)&&
  190. (lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()>0))
  191. {
  192. int xp = (int)((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()/0.2)/2.0);
  193. if(xp == 0)xp=1;
  194. int yp = (int)((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()/0.2)/2.0);
  195. if(yp == 0)yp=1;
  196. int ix,iy;
  197. for(ix = 0; ix<(xp*2); ix++)
  198. {
  199. for(iy = 0; iy<(yp*2); iy++)
  200. {
  201. iv::fusion::NomalXYZ nomal_forcast;
  202. iv::fusion::NomalXYZ *nomal_forcast_;
  203. float nomal_x = ix*0.2 - xp*0.2;
  204. float nomal_y = iy*0.2 - yp*0.2;
  205. float nomal_z = lidar_object_arr.obj(match_idx[i].nlidar).centroid().z();
  206. float s = nomal_x*cos(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()) -
  207. nomal_y*sin(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
  208. float t = nomal_x*sin(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()) +
  209. nomal_y*cos(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
  210. nomal_forcast.set_nomal_x(forcast_x + s);
  211. nomal_forcast.set_nomal_y(forcast_y + t);
  212. nomal_forcast_ = forcast_normal.add_forecast_nomal();
  213. nomal_forcast_->CopyFrom(nomal_forcast);
  214. }
  215. }
  216. }
  217. forcast_normal_=fusion_object.add_forecast_nomals();
  218. forcast_normal_->CopyFrom(forcast_normal);
  219. }
  220. iv::fusion::fusionobject *pobj = lidar_radar_fusion_object_array.add_obj();
  221. pobj->CopyFrom(fusion_object);
  222. }
  223. for(int j = 0; j< radar_idx.size() ; j++){
  224. iv::fusion::fusionobject fusion_obj;
  225. fusion_obj.set_yaw(radar_object_array.obj(radar_idx[j]).angle());
  226. iv::fusion::VelXY vel_relative;
  227. iv::fusion::VelXY *vel_relative_;
  228. vel_relative.set_x(radar_object_array.obj(radar_idx[j]).vx());
  229. vel_relative.set_y(radar_object_array.obj(radar_idx[j]).vy());
  230. vel_relative_ = fusion_obj.mutable_vel_relative();
  231. vel_relative_->CopyFrom(vel_relative);
  232. iv::fusion::PointXYZ centroid;
  233. iv::fusion::PointXYZ *centroid_;
  234. centroid.set_x(radar_object_array.obj(radar_idx[j]).x());
  235. centroid.set_y(radar_object_array.obj(radar_idx[j]).y());
  236. centroid.set_z(1.0);
  237. centroid_ = fusion_obj.mutable_centroid();
  238. centroid_->CopyFrom(centroid);
  239. iv::fusion::Dimension dimension;
  240. iv::fusion::Dimension *dimension_;
  241. dimension.set_x(0.5);
  242. dimension.set_y(0.5);
  243. dimension.set_z(1.0);
  244. dimension_ = fusion_obj.mutable_dimensions();
  245. dimension_->CopyFrom(dimension);
  246. int xp = (int)((0.5/0.2)/2.0);
  247. if(xp == 0)xp=1;
  248. int yp = (int)((0.5/0.2)/2.0);
  249. if(yp == 0)yp=1;
  250. int ix,iy;
  251. for(ix = 0; ix<(xp*2); ix++)
  252. {
  253. for(iy = 0; iy<(yp*2); iy++)
  254. {
  255. iv::fusion::NomalXYZ nomal_centroid;
  256. iv::fusion::NomalXYZ *nomal_centroid_;
  257. float nomal_x = ix*0.2 - xp*0.2;
  258. float nomal_y = iy*0.2 - yp*0.2;
  259. float nomal_z = 1.0;
  260. float s = nomal_x*cos(radar_object_array.obj(radar_idx[j]).angle())
  261. - nomal_y*sin(radar_object_array.obj(radar_idx[j]).angle());
  262. float t = nomal_x*sin(radar_object_array.obj(radar_idx[j]).angle())
  263. + nomal_y*cos(radar_object_array.obj(radar_idx[j]).angle());
  264. nomal_centroid.set_nomal_x(radar_object_array.obj(radar_idx[j]).x() + s);
  265. nomal_centroid.set_nomal_y(radar_object_array.obj(radar_idx[j]).y() + t);
  266. nomal_centroid_ = fusion_obj.add_nomal_centroid();
  267. nomal_centroid_->CopyFrom(nomal_centroid);
  268. }
  269. }
  270. for(int k = 0; k<10; k++)
  271. {
  272. // std::cout<<"fusion begin"<<std::endl;
  273. iv::fusion::PointXYZ point_forcaste;
  274. iv::fusion::PointXYZ *point_forcaste_;
  275. float forcast_x = radar_object_array.obj(radar_idx[j]).x()
  276. + radar_object_array.obj(radar_idx[j]).vx()*time_step*(k+1);
  277. float forcast_y = radar_object_array.obj(radar_idx[j]).y()
  278. + radar_object_array.obj(radar_idx[j]).vy()*time_step*(k+1);
  279. point_forcaste.set_x(forcast_x);
  280. point_forcaste.set_y(forcast_y);
  281. point_forcaste.set_z(1.0);
  282. point_forcaste_ = fusion_obj.add_point_forecast();
  283. point_forcaste_->CopyFrom(point_forcaste);
  284. iv::fusion::NomalForecast forcast_normal;
  285. iv::fusion::NomalForecast *forcast_normal_;
  286. forcast_normal.set_forecast_index(k);
  287. int xp = (int)((0.5/0.2)/2.0);
  288. if(xp == 0)xp=1;
  289. int yp = (int)((0.5/0.2)/2.0);
  290. if(yp == 0)yp=1;
  291. int ix,iy;
  292. for(ix = 0; ix<(xp*2); ix++)
  293. {
  294. for(iy = 0; iy<(yp*2); iy++)
  295. {
  296. iv::fusion::NomalXYZ nomal_forcast;
  297. iv::fusion::NomalXYZ *nomal_forcast_;
  298. float nomal_x = ix*0.2 - xp*0.2;
  299. float nomal_y = iy*0.2 - yp*0.2;
  300. float nomal_z = 1.0;
  301. float s = nomal_x*cos(radar_object_array.obj(radar_idx[j]).angle())
  302. - nomal_y*sin(radar_object_array.obj(radar_idx[j]).angle());
  303. float t = nomal_x*sin(radar_object_array.obj(radar_idx[j]).angle())
  304. + nomal_y*cos(radar_object_array.obj(radar_idx[j]).angle());
  305. nomal_forcast.set_nomal_x(forcast_x + s);
  306. nomal_forcast.set_nomal_y(forcast_y + t);
  307. nomal_forcast_ = forcast_normal.add_forecast_nomal();
  308. nomal_forcast_->CopyFrom(nomal_forcast);
  309. }
  310. }
  311. forcast_normal_=fusion_obj.add_forecast_nomals();
  312. forcast_normal_->CopyFrom(forcast_normal);
  313. }
  314. iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
  315. obj->CopyFrom(fusion_obj);
  316. }
  317. // std::cout<<" fusion end "<<std::endl;
  318. }
  319. void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array, iv::mobileye::mobileye& xobs_info)
  320. {
  321. int time_step = 0.3;
  322. lidar_radar_fusion_object_array.Clear();
  323. for(int j = 0; j< xobs_info.xobj_size() ; j++){
  324. iv::fusion::fusionobject fusion_obj;
  325. fusion_obj.set_yaw(xobs_info.xobj(j).obsang());
  326. iv::fusion::VelXY vel_relative;
  327. iv::fusion::VelXY *vel_relative_;
  328. vel_relative.set_x(xobs_info.xobj(j).obs_rel_vel_x() * tan(xobs_info.xobj(j).obsang()));
  329. vel_relative.set_y(xobs_info.xobj(j).obs_rel_vel_x());
  330. vel_relative_ = fusion_obj.mutable_vel_relative();
  331. vel_relative_->CopyFrom(vel_relative);
  332. iv::fusion::PointXYZ centroid;
  333. iv::fusion::PointXYZ *centroid_;
  334. centroid.set_x(-(xobs_info.xobj(j).pos_y()));
  335. centroid.set_y(xobs_info.xobj(j).pos_x());
  336. centroid.set_z(1.0);
  337. centroid_ = fusion_obj.mutable_centroid();
  338. centroid_->CopyFrom(centroid);
  339. iv::fusion::Dimension dimension;
  340. iv::fusion::Dimension *dimension_;
  341. dimension.set_x(xobs_info.xobj(j).obswidth());
  342. dimension.set_y(2.0);
  343. dimension.set_z(1.0);
  344. dimension_ = fusion_obj.mutable_dimensions();
  345. dimension_->CopyFrom(dimension);
  346. int xp = (int)((xobs_info.xobj(j).obswidth()/0.2)/2.0);
  347. if(xp == 0)xp=1;
  348. int yp = (int)((2.0/0.2)/2.0);
  349. if(yp == 0)yp=1;
  350. int ix,iy;
  351. for(ix = 0; ix<(xp*2); ix++)
  352. {
  353. for(iy = 0; iy<(yp*2); iy++)
  354. {
  355. iv::fusion::NomalXYZ nomal_centroid;
  356. iv::fusion::NomalXYZ *nomal_centroid_;
  357. float nomal_x = ix*0.2 - xp*0.2;
  358. float nomal_y = iy*0.2 - yp*0.2;
  359. float nomal_z = 1.0;
  360. float s = nomal_x*cos(0)
  361. - nomal_y*sin(0);
  362. float t = nomal_x*sin(0)
  363. + nomal_y*cos(0);
  364. nomal_centroid.set_nomal_x(-xobs_info.xobj(j).pos_y() + s);
  365. nomal_centroid.set_nomal_y(xobs_info.xobj(j).pos_x() + t);
  366. nomal_centroid_ = fusion_obj.add_nomal_centroid();
  367. nomal_centroid_->CopyFrom(nomal_centroid);
  368. }
  369. }
  370. iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
  371. obj->CopyFrom(fusion_obj);
  372. }
  373. }
  374. }
  375. }
  376. #endif // FUSION_HPP