modify fusion

src/fusion/lidar_radar_fusion_cnn/fusion.hpp

@@ -147,15 +147,6 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
             centroid_->CopyFrom(centroid);
         }
 
-        iv::fusion::PointXYZ min_point;
-        iv::fusion::PointXYZ *min_point_;
-        min_point.set_x(lidar_object_arr.obj(match_idx[i].nlidar).position().x()
-                        - lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()/2.0);
-        min_point.set_y(lidar_object_arr.obj(match_idx[i].nlidar).position().y()
-                        - lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()/2.0);
-        min_point_ = fusion_object.mutable_min_point();
-        min_point_->CopyFrom(min_point);
-
         iv::fusion::Dimension dimension;
         iv::fusion::Dimension *dimension_;
         dimension.set_x(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x());
@@ -164,8 +155,8 @@ 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).centroid().x()>0)&&
-                (lidar_object_arr.obj(match_idx[i].nlidar).centroid().y()>0))
+        if((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()>0)&&
+                (lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()>0))
         {
             int xp = (int)((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()/0.2)/2.0);
             if(xp == 0)xp=1;
@@ -181,10 +172,10 @@ 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());
+                    float s = nomal_x*cos(0) -
+                            nomal_y*sin(0);
+                    float t = nomal_x*sin(0) +
+                            nomal_y*cos(0);
                     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();

src/fusion/lidar_radar_fusion_cnn/main.cpp

@@ -109,14 +109,14 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
     RLfusion(lidar_obj,radarobjvec,li_ra_fusion);
 
     AddMobileye(li_ra_fusion,mobileye_info);
-    for(int i=0;i<li_ra_fusion.obj_size();i++)
-    {
-        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;
-    }
+//    for(int i=0;i<li_ra_fusion.obj_size();i++)
+//    {
+//        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;
+//    }
 
 
 //    int nsize =0;

src/fusion/lidar_radar_fusion_cnn_ukf/fusion.hpp

@@ -175,6 +175,47 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
                                           "________________________"
                 <<lidar_object_arr.obj(match_idx[i].nlidar).tyaw()<<"    "<<
                lidar_object_arr.obj(match_idx[i].nlidar).velocity_linear_x()<<std::endl;
+
+        int lx,ly;
+        if(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x() > lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y())
+        {
+            lx = lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y();
+            ly = lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x();
+        } else {
+            lx = lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x();
+            ly = lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y();
+        }
+
+
+        if((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()>0)&&
+                      (lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()>0))
+              {
+                  int xp = (int)((lx/0.2)/2.0);
+                  if(xp == 0)xp=1;
+                  int yp = (int)((ly/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 = lidar_object_arr.obj(match_idx[i].nlidar).centroid().z();
+                          float s = nomal_x*cos(0) -
+                                  nomal_y*sin(0);
+                          float t = nomal_x*sin(0) +
+                                  nomal_y*cos(0);
+                          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);
+                      }
+                  }
+              }
+
         iv::fusion::fusionobject *pobj = lidar_radar_fusion_object_array.add_obj();
         pobj->CopyFrom(fusion_object);
     }
@@ -203,6 +244,32 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
         dimension.set_z(1.0);
         dimension_ = fusion_obj.mutable_dimensions();
         dimension_->CopyFrom(dimension);
+
+        int xp = (int)((0.5/0.2)/2.0);
+               if(xp == 0)xp=1;
+               int yp = (int)((0.5/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(radar_object_array.obj(radar_idx[j]).angle())
+                               - nomal_y*sin(radar_object_array.obj(radar_idx[j]).angle());
+                       float t = nomal_x*sin(radar_object_array.obj(radar_idx[j]).angle())
+                               + nomal_y*cos(radar_object_array.obj(radar_idx[j]).angle());
+                       nomal_centroid.set_nomal_x(radar_object_array.obj(radar_idx[j]).x() + s);
+                       nomal_centroid.set_nomal_y(radar_object_array.obj(radar_idx[j]).y() + t);
+                       nomal_centroid_ = fusion_obj.add_nomal_centroid();
+                       nomal_centroid_->CopyFrom(nomal_centroid);
+                   }
+               }
+
         iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
         obj->CopyFrom(fusion_obj);
     }
@@ -255,6 +322,31 @@ void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array,
         dimension_ = fusion_obj.mutable_dimensions();
         dimension_->CopyFrom(dimension);
 
+        int xp = (int)((xobs_info.xobj(j).obswidth()/0.2)/2.0);
+         if(xp == 0)xp=1;
+         int yp = (int)((2.0/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(0)
+                         - nomal_y*sin(0);
+                 float t = nomal_x*sin(0)
+                         + nomal_y*cos(0);
+                 nomal_centroid.set_nomal_x(-xobs_info.xobj(j).pos_y() + s);
+                 nomal_centroid.set_nomal_y(xobs_info.xobj(j).pos_x() + t);
+                 nomal_centroid_ = fusion_obj.add_nomal_centroid();
+                 nomal_centroid_->CopyFrom(nomal_centroid);
+             }
+         }
+
         iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
         obj->CopyFrom(fusion_obj);
     }

src/fusion/lidar_radar_fusion_cnn_ukf/main.cpp

@@ -101,32 +101,32 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
 //    Transformation::RadarPross(radarobjvec);
     RLfusion(lidar_obj,radarobjvec,li_ra_fusion);
     AddMobileye(li_ra_fusion,mobileye_info);
-    giup.call(li_ra_fusion);
-
-
-//    string out;
-
-//    if(li_ra_fusion.obj_size() == 0)
-//    {
-//        iv::fusion::fusionobject fake_obj;
-//        iv::fusion::fusionobject *fake_obj_;
-//        iv::fusion::PointXYZ fake_cen;
-//        iv::fusion::PointXYZ *fake_cen_;
-//        fake_cen.set_x(10000);
-//        fake_cen.set_y(10000);
-//        fake_cen.set_z(10000);
-//        fake_cen_ = fake_obj.mutable_centroid();
-//        fake_cen_ ->CopyFrom(fake_cen);
-
-//        fake_obj_ = li_ra_fusion.add_obj();
-//        fake_obj_->CopyFrom(fake_obj);
-//        out = li_ra_fusion.SerializeAsString();
-//    }else
-//    {
-//        out = li_ra_fusion.SerializeAsString();
-//    }
-//    iv::modulecomm::ModuleSendMsg(gfu,out.data(),out.length());
-////    gMutex.unlock();
+//    giup.call(li_ra_fusion);
+
+
+    string out;
+
+    if(li_ra_fusion.obj_size() == 0)
+    {
+        iv::fusion::fusionobject fake_obj;
+        iv::fusion::fusionobject *fake_obj_;
+        iv::fusion::PointXYZ fake_cen;
+        iv::fusion::PointXYZ *fake_cen_;
+        fake_cen.set_x(10000);
+        fake_cen.set_y(10000);
+        fake_cen.set_z(10000);
+        fake_cen_ = fake_obj.mutable_centroid();
+        fake_cen_ ->CopyFrom(fake_cen);
+
+        fake_obj_ = li_ra_fusion.add_obj();
+        fake_obj_->CopyFrom(fake_obj);
+        out = li_ra_fusion.SerializeAsString();
+    }else
+    {
+        out = li_ra_fusion.SerializeAsString();
+    }
+    iv::modulecomm::ModuleSendMsg(gpatrack,out.data(),out.length());
+//    gMutex.unlock();
 }