fusion.hpp 19 KB

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