fusion modified

src/fusion/lidar_radar_fusion_cnn/fusion.hpp

@@ -13,7 +13,7 @@ namespace iv {
 namespace fusion {
 
 //std::vector<Match_index> match_idxs;
-
+8
 static float time_step = 0.3;
 static std::vector<std::string> labels = {"unknown","ped","bike","car","bus_or_truck"};
 
@@ -105,18 +105,19 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
         {
 //            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(0);
-            vel_relative.set_y(2);
+            vel_relative.set_y(-2);
             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.set_x(lidar_object_arr.obj(match_idx[i].nlidar).position().x());
+            centroid.set_y(lidar_object_arr.obj(match_idx[i].nlidar).position().y());
+            centroid.set_z(lidar_object_arr.obj(match_idx[i].nlidar).position().z());
             centroid_ = fusion_object.mutable_centroid();
             centroid_->CopyFrom(centroid);
         }else {
@@ -126,8 +127,10 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
 
             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.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.set_x(0);
+            vel_relative.set_y(2);
             vel_relative_ = fusion_object.mutable_vel_relative();
             vel_relative_->CopyFrom(vel_relative);
 
@@ -155,7 +158,19 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
         dimension_ = fusion_object.mutable_dimensions();
         dimension_->CopyFrom(dimension);
 
-        if(lidar_object_arr.obj(match_idx[i].nlidar).position().y() > 10 && abs(lidar_object_arr.obj(match_idx[i].nlidar).position().x())<4 ) continue;
+        if( lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x() *
+               lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y() < 0.25)
+        {
+            fusion_object.set_type(2);
+        } else {
+            fusion_object.set_type(1);
+        }
+
+
+
+        if(lidar_object_arr.obj(match_idx[i].nlidar).position().y() > 20 || abs(lidar_object_arr.obj(match_idx[i].nlidar).position().x())>10 ) continue;
+        if (lidar_object_arr.obj(match_idx[i].nlidar).position().y()>10 && abs(lidar_object_arr.obj(match_idx[i].nlidar).position().x())<1.3) continue;
+        if(lidar_object_arr.obj(match_idx[i].nlidar).position().y() <1.0  && abs(lidar_object_arr.obj(match_idx[i].nlidar).position().x())<1.3) continue;
         if((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()>0)&&
                 (lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()>0))
         {
@@ -164,6 +179,14 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
             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;
+           float yaw;
+           if( lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x() *
+                  lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y() > 8)
+           {
+               yaw = 1.57;
+           } else {
+               yaw = lidar_object_arr.obj(match_idx[i].nlidar).tyaw();
+           }
             for(ix = 0; ix<(xp*2); ix++)
             {
                 for(iy = 0; iy<(yp*2); iy++)
@@ -173,14 +196,18 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
                     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_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);
+                    float s = nomal_x*cos(yaw) -
+                            nomal_y*sin(yaw);
+                    float t = nomal_x*sin(yaw) +
+                            nomal_y*cos(yaw);
+                    if(abs(lidar_object_arr.obj(match_idx[i].nlidar).centroid().x() + s) <1.3 &&
+                            lidar_object_arr.obj(match_idx[i].nlidar).centroid().y() + t <1.0) continue;
+                    else{
+                        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);
+                    }
                 }
             }
         }
@@ -189,6 +216,7 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
         pobj->CopyFrom(fusion_object);
     }
     for(int j = 0; j< radar_idx.size() ; j++){
+        break;
         iv::fusion::fusionobject fusion_obj;
         fusion_obj.set_yaw(radar_object_array.obj(radar_idx[j]).angle());
         iv::fusion::VelXY vel_relative;
@@ -255,8 +283,14 @@ void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array,
         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());
+        if(xobs_info.xobj(j).obstype() == iv::mobileye::obs_OBSTYPE::obs_OBSTYPE_PED)
+        {
+            fusion_obj.set_type(2);
+        }else{
+            fusion_obj.set_type(1);
+            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);
 

src/fusion/lidar_radar_fusion_cnn/main.cpp

@@ -58,6 +58,19 @@ void Listenesrfront(const char * strdata,const unsigned int nSize,const unsigned
     }
 //        std::cout<<"radar is ok :  "<<radarobj.obj_size()<<std::endl;
     gMutex.lock();
+//    for(int i=0;i<radarobj.obj_size();i++)
+//    {
+//        if(abs(radarobj.obj(i).x())<2 && abs(radarobj.obj(i).vy()) >10)
+//        {
+//            std::cout<<"x    y      vx    vy  "<<radarobj.obj(i).x()<<"          "
+//                    <<radarobj.obj(i).y()<<"         "
+//                    <<radarobj.obj(i).vx()<<"     "
+//                    <<radarobj.obj(i).vy()<<"    "<<std::endl;
+
+//        }
+
+//    }
+
     radarobjvec.CopyFrom(radarobj);
     gMutex.unlock();
 }
@@ -120,12 +133,16 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
 //        std::cout<< "     fusion size  "<<li_ra_fusion.obj_size()<<std::endl;
 //    for(int i=0;i<li_ra_fusion.obj_size();i++)
 //    {
-//        std::cout<<"  is  ok  "<<std::endl;
-//        std::cout<<"x    y      vx    vy   w "<<li_ra_fusion.obj(i).centroid().x()<<"          "
-//                <<li_ra_fusion.obj(i).centroid().y()<<"         "
-//                <<li_ra_fusion.obj(i).vel_relative().x()<<"     "
-//                <<li_ra_fusion.obj(i).vel_relative().y()<<"     "
-//                <<li_ra_fusion.obj(i).dimensions().x()<<std::endl;
+//        if(abs(li_ra_fusion.obj(i).vel_relative().y())>10 )
+//        {
+//            std::cout<<"x    y      vx    vy   w "<<li_ra_fusion.-obj(i).centroid().x()<<"          "
+//                    <<li_ra_fusion.obj(i).centroid().y()<<"         "
+//                    <<li_ra_fusion.obj(i).vel_relative().x()<<"     "
+//                    <<li_ra_fusion.obj(i).vel_relative().y()<<"     "
+//                    <<li_ra_fusion.obj(i).dimensions().x()<<std::endl;
+
+//        }
+
 //    }