1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798 |
- #include "fusion_probabilities.h"
- int iv::fusion::FusionProbabilities::ComputePointInBox(cv::Point2f PointInCamera, Point top_left, Point bot_right)
- {
- if(!((PointInCamera.x>=top_left.x)&&(PointInCamera.x<=bot_right.x)&&(PointInCamera.y>=bot_right.y)&&(PointInCamera.y<=top_left.y)))
- {
- return 0;
- } else {
- return 1;
- }
- }
- int iv::fusion::FusionProbabilities::ComputeLidarCameraFusionProb(const iv::fusion::fusionobject& li_ra_fusion_object, const iv::vision::Bbox3D& vision_object )
- {
- std::vector<cv::Point2f> lidarIncamera;
- int flag = 0;
- Point top_left, bot_right;
- top_left.x = vision_object.top_left_x();
- top_left.y = vision_object.top_left_y();
- bot_right.x = vision_object.bottom_right_x();
- bot_right.y = vision_object.bottom_right_y();
- iv::fusion::Transformation::PointInCamera( li_ra_fusion_object,lidarIncamera);
- for(std::vector<cv::Point2f>::iterator point2D = lidarIncamera.begin(); point2D != lidarIncamera.end(); point2D++)
- {
- flag = flag + ComputePointInBox(*point2D, top_left,bot_right);
- }
- lidarIncamera.clear();
- return flag;
- }
- //int iv::fusion::FusionProbabilities::ComputeRadarInBox(Point radarincamera, Point top_left, Point bot_right)
- //{
- // if(!((radarincamera.x>=top_left.x)&&(radarincamera.x<=bot_right.x)&&(radarincamera.y>=bot_right.y)&&(radarincamera.y<=top_left.y)))
- // {
- // return 0;
- // } else {
- // return 1;
- // }
- //}
- //int iv::fusion::FusionProbabilities::ComputeRadarCameraFusionProb(const iv::radar::radarobject &radar_object, const iv::vision::cameraobject &camera_object)
- //{
- // double z = 0.3;
- // int flag = 0;
- // Point top_left, bot_right;
- // top_left.x = camera_object.x() - int(camera_object.w()/2);
- // top_left.y = camera_object.y() + int(camera_object.h()/2);
- // bot_right.x = camera_object.x() + int(camera_object.w()/2);
- // bot_right.y = camera_object.y() - int(camera_object.h()/2);
- // for(int i = 0; i<5; i++)
- // {
- // Point radar_in_world, radar_in_camera;
- // radar_in_world.x = radar_object.x();
- // radar_in_world.y = radar_object.y();
- // radar_in_world.z = z*(i+1);
- // radar_in_camera = iv::fusion::Transformation::RadarToCamera(radar_in_world);
- // flag = flag + iv::fusion::FusionProbabilities::ComputeRadarInBox(radar_in_camera, top_left, bot_right);
- // }
- // if(flag>=3)
- // {
- // return 1;
- // }else {
- // return 0;
- // }
- //}
- //毫米波雷达object点是否和激光雷达object的俯视box匹配
- int iv::fusion::FusionProbabilities::ComputRadarLidarmatch(const iv::radar::radarobject& radarPoint, const iv::lidar::object& lidarobject)
- {
- if(!((radarPoint.x()>=lidarobject.max_point().x())&&(radarPoint.x()<= lidarobject.min_point().x())&&
- ((radarPoint.y() - 2)>=lidarobject.max_point().y())&&((radarPoint.y() - 2)<=lidarobject.min_point().y())))
- {
- return 0;
- } else {
- return 1;
- }
- }
|