提交车端最新的代码到仓库

src/detection/detection_lidar_PointPillars_MultiHead/main.cpp

@@ -165,6 +165,8 @@ void GetLidarObj(std::vector<BBOX3D> &results,iv::lidar::objectarray & lidarobjv
         float yaw = out_detection.at(6);
         yaw += M_PI / 2;
         yaw = std::atan2(sinf(yaw),cosf(yaw));
+        yaw = -yaw;
+        yaw = yaw + M_PI /2;
         lidarobj.set_tyaw(yaw);
         iv::lidar::PointXYZ centroid;
         iv::lidar::PointXYZ * _centroid;

src/driver/driver_cloud_grpc_client_BS/driver_cloud_grpc_client_BS.yaml

@@ -1,4 +1,4 @@
-server : 111.33.136.150
+server : 61.130.104.221
 upload_port : 10591
 patrol_port : 10592
 control_port : 20591

src/fusion/lidar_radar_fusion_cnn/fusion.hpp

@@ -17,6 +17,58 @@ namespace fusion {
 static float time_step = 0.3;
 static std::vector<std::string> labels = {"unknown","ped","bike","car","bus_or_truck"};
 
+void ObsToNormal(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
+{
+//    int cont = 0;
+    for(int i = 0; i < lidar_radar_fusion_object_array.obj_size(); i++)
+    {
+//        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;
+//        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;
+        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)
+                || lidar_radar_fusion_object_array.obj(i).centroid().y() <-1.0) continue;
+
+        if((lidar_radar_fusion_object_array.obj(i).dimensions().x()>0)&&
+                (lidar_radar_fusion_object_array.obj(i).dimensions().y()>0))
+        {
+            int xp = (int)((lidar_radar_fusion_object_array.obj(i).dimensions().x()/0.2)/2.0);
+            if(xp == 0)xp=1;
+            int yp = (int)((lidar_radar_fusion_object_array.obj(i).dimensions().y()/0.2)/2.0);
+            if(yp == 0)yp=1;
+            int ix,iy;
+            for(ix = 0; ix<(xp*2); ix++)
+            {
+                for(iy = 0; iy<(yp*2); iy++)
+                {
+                    iv::fusion::NomalXYZ nomal_centroid;
+                    iv::fusion::NomalXYZ *nomal_centroid_;
+                    float nomal_x = ix*0.2 - xp*0.2;
+                    float nomal_y = iy*0.2 - yp*0.2;
+                    float nomal_z = 1.0;
+                    float s = nomal_x*cos(lidar_radar_fusion_object_array.obj(i).yaw())
+                            - nomal_y*sin(lidar_radar_fusion_object_array.obj(i).yaw());
+                    float t = nomal_x*sin(lidar_radar_fusion_object_array.obj(i).yaw())
+                            + nomal_y*cos(lidar_radar_fusion_object_array.obj(i).yaw());
+                    nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s);
+                    nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t);
+                    if(abs(lidar_radar_fusion_object_array.obj(i).centroid().x() + s) <1.3 &&
+                            (lidar_radar_fusion_object_array.obj(i).centroid().y() + t) <1.0 &&
+                            (lidar_radar_fusion_object_array.obj(i).centroid().y() + t) >-1.0) continue;
+                    else{
+
+                        nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s);
+                        nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t);
+                        iv::fusion::fusionobject &fusion_obj = (iv::fusion::fusionobject &)lidar_radar_fusion_object_array.obj(i);
+                        nomal_centroid_ = fusion_obj.add_nomal_centroid();
+                        nomal_centroid_->CopyFrom(nomal_centroid);
+                    }
+                }
+            }
+
+    }
+}
+//    std::cout<<"   count    "<<cont<<std::endl;
+}
+
 float ComputeDis(cv::Point2f po1, cv::Point2f po2)
 {
     return (sqrt(pow((po1.x-po2.x),2) + pow((po1.y-po2.y),2)));
@@ -215,6 +267,7 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
         iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
         obj->CopyFrom(fusion_obj);
     }
+        ObsToNormal(lidar_radar_fusion_object_array);
 //    std::cout<<"   fusion   end    "<<std::endl;
 }
 
@@ -254,58 +307,6 @@ void AddRadarBack(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array
 }
 }
 
-void ObsToNormal(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
-{
-//    int cont = 0;
-    for(int i = 0; i < lidar_radar_fusion_object_array.obj_size(); i++)
-    {
-//        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;
-//        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;
-        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)
-                || lidar_radar_fusion_object_array.obj(i).centroid().y() <-1.0) continue;
-
-        if((lidar_radar_fusion_object_array.obj(i).dimensions().x()>0)&&
-                (lidar_radar_fusion_object_array.obj(i).dimensions().y()>0))
-        {
-            int xp = (int)((lidar_radar_fusion_object_array.obj(i).dimensions().x()/0.2)/2.0);
-            if(xp == 0)xp=1;
-            int yp = (int)((lidar_radar_fusion_object_array.obj(i).dimensions().y()/0.2)/2.0);
-            if(yp == 0)yp=1;
-            int ix,iy;
-            for(ix = 0; ix<(xp*2); ix++)
-            {
-                for(iy = 0; iy<(yp*2); iy++)
-                {
-                    iv::fusion::NomalXYZ nomal_centroid;
-                    iv::fusion::NomalXYZ *nomal_centroid_;
-                    float nomal_x = ix*0.2 - xp*0.2;
-                    float nomal_y = iy*0.2 - yp*0.2;
-                    float nomal_z = 1.0;
-                    float s = nomal_x*cos(lidar_radar_fusion_object_array.obj(i).yaw())
-                            - nomal_y*sin(lidar_radar_fusion_object_array.obj(i).yaw());
-                    float t = nomal_x*sin(lidar_radar_fusion_object_array.obj(i).yaw())
-                            + nomal_y*cos(lidar_radar_fusion_object_array.obj(i).yaw());
-                    nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s);
-                    nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t);
-                    if(abs(lidar_radar_fusion_object_array.obj(i).centroid().x() + s) <1.3 &&
-                            (lidar_radar_fusion_object_array.obj(i).centroid().y() + t) <1.0 &&
-                            (lidar_radar_fusion_object_array.obj(i).centroid().y() + t) >-1.0) continue;
-                    else{
-
-                        nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s);
-                        nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t);
-                        iv::fusion::fusionobject &fusion_obj = (iv::fusion::fusionobject &)lidar_radar_fusion_object_array.obj(i);
-                        nomal_centroid_ = fusion_obj.add_nomal_centroid();
-                        nomal_centroid_->CopyFrom(nomal_centroid);
-                    }
-                }
-            }
-
-    }
-}
-//    std::cout<<"   count    "<<cont<<std::endl;
-}
-
 }
 }
 

src/fusion/lidar_radar_fusion_cnn/main.cpp

@@ -170,7 +170,7 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
     //--------------------------------------------  end tracking  --------------------------------------------------
     //    gMutex.unlock();
 //    iv::fusion::fusionobjectarray out_fusion = trackedobjvec;
-    ObsToNormal(li_ra_fusion);
+
     string out;
 
     if(li_ra_fusion.obj_size() == 0)
@@ -196,12 +196,12 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
 
     }
 
-        for (int k = 0; k<li_ra_fusion.obj_size();k++){
-            if(abs(li_ra_fusion.obj(k).centroid().x())<1 && li_ra_fusion.obj(k).centroid().y()>5 ){
-            std::cout<<" size   x    y    vx   vy :   "<<li_ra_fusion.obj(k).centroid().x()<< "   "<<li_ra_fusion.obj(k).centroid().y()<<"    "
-                   <<li_ra_fusion.obj(k).velocity_linear_x()<<std::endl;
-        }
-        }
+//        for (int k = 0; k<li_ra_fusion.obj_size();k++){
+//            if(abs(li_ra_fusion.obj(k).centroid().x())<1 && li_ra_fusion.obj(k).centroid().y()>5 ){
+//            std::cout<<" size   x    y    vx   vy :   "<<li_ra_fusion.obj(k).centroid().x()<< "   "<<li_ra_fusion.obj(k).centroid().y()<<"    "
+//                   <<li_ra_fusion.obj(k).velocity_linear_x()<<std::endl;
+//        }
+//        }
 
     iv::modulecomm::ModuleSendMsg(gfu,out.data(),out.length());
     //    gMutex.unlock();