fusion lidar....

src/fusion/lidar_radar_fusion_cnn_ukf/fusion.hpp

@@ -104,11 +104,11 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
         if(match_idx.at(i).nradar == -1000)
         {
 //            std::cout<<"   fusion    is    ok  "<<std::endl;
-            fusion_object.set_yaw(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
+            fusion_object.set_yaw(lidar_object_arr.obj(match_idx[i].nlidar).angle());
             iv::fusion::VelXY vel_relative;
             iv::fusion::VelXY *vel_relative_;
-            vel_relative.set_x(lidar_object_arr.obj(match_idx[i].nlidar).position().x());
-            vel_relative.set_y(lidar_object_arr.obj(match_idx[i].nlidar).position().y());
+            vel_relative.set_x(lidar_object_arr.obj(match_idx[i].nlidar).velocity_linear_x());
+            vel_relative.set_y(lidar_object_arr.obj(match_idx[i].nlidar).velocity_linear_x()*tan(lidar_object_arr.obj(match_idx[i].nlidar).angle()));
             vel_relative_ = fusion_object.mutable_vel_relative();
             vel_relative_->CopyFrom(vel_relative);
 
@@ -158,48 +158,23 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
 
         iv::fusion::Dimension dimension;
         iv::fusion::Dimension *dimension_;
-        if(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x() > lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()){
-             dimension.set_x(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y());
-             dimension.set_y(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x());
-        } else {
-            dimension.set_x(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x());
-            dimension.set_y(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y());
-        }
+//        if(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x() > lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()){
+//             dimension.set_x(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y());
+//             dimension.set_y(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x());
+//        } else {
+//            dimension.set_x(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x());
+//            dimension.set_y(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y());
+//        }
+        dimension.set_x(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x());
+        dimension.set_y(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y());
         dimension.set_z(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().z());
         dimension_ = fusion_object.mutable_dimensions();
         dimension_->CopyFrom(dimension);
 
-        std::cout<<" tyaw       angle   "<<lidar_object_arr.obj(match_idx[i].nlidar).angle()<<" __"
-                                          "________________________"<<lidar_object_arr.obj(match_idx[i].nlidar).tyaw()<<std::endl;
-
-        if((lidar_object_arr.obj(match_idx[i].nlidar).centroid().x()>0)&&
-                (lidar_object_arr.obj(match_idx[i].nlidar).centroid().y()>0))
-        {
-            int xp = (int)((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()/0.2)/2.0);
-            if(xp == 0)xp=1;
-            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;
-            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(lidar_object_arr.obj(match_idx[i].nlidar).angle()) -
-                            nomal_y*sin(lidar_object_arr.obj(match_idx[i].nlidar).angle());
-                    float t = nomal_x*sin(lidar_object_arr.obj(match_idx[i].nlidar).angle()) +
-                            nomal_y*cos(lidar_object_arr.obj(match_idx[i].nlidar).angle());
-                    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);
-                }
-            }
-        }
+        std::cout<<" tyaw       angle    "<<lidar_object_arr.obj(match_idx[i].nlidar).angle()<<" __"
+                                          "________________________"
+                <<lidar_object_arr.obj(match_idx[i].nlidar).tyaw()<<"    "<<
+               lidar_object_arr.obj(match_idx[i].nlidar).velocity_linear_x()<<std::endl;
         iv::fusion::fusionobject *pobj = lidar_radar_fusion_object_array.add_obj();
         pobj->CopyFrom(fusion_object);
     }
@@ -228,31 +203,6 @@ 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);
     }
@@ -266,7 +216,7 @@ void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array,
     int time_step = 0.3;
     for(int j = 0; j< xobs_info.xobj_size() ; j++){
         iv::fusion::fusionobject fusion_obj;
-        fusion_obj.set_yaw(xobs_info.xobj(j).obsang());
+        fusion_obj.set_yaw(0);
         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()));
@@ -285,48 +235,26 @@ void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array,
 
         iv::fusion::Dimension dimension;
         iv::fusion::Dimension *dimension_;
-        if (xobs_info.xobj(j).obswidth() >= 2.5)
-        {
-          dimension.set_x(2.5);
-          if (xobs_info.xobj(j).obswidth() >= 5.0)
-          {
-            dimension.set_y(5.0);
-          }else {
-             dimension.set_y(xobs_info.xobj(j).obswidth());
-            }
-        }else {
-          dimension.set_x(xobs_info.xobj(j).obswidth());
-          dimension.set_y(2.5);
-        }
+//        if (xobs_info.xobj(j).obswidth() >= 2.5)
+//        {
+//          dimension.set_x(2.5);
+//          if (xobs_info.xobj(j).obswidth() >= 5.0)
+//          {
+//            dimension.set_y(5.0);
+//          }else {
+//             dimension.set_y(xobs_info.xobj(j).obswidth());
+//            }
+//        }else {
+//          dimension.set_x(xobs_info.xobj(j).obswidth());
+//          dimension.set_y(2.5);
+//        }
+        dimension.set_x(xobs_info.xobj(j).obswidth());
+        dimension.set_y(2.5);
+
         dimension.set_z(1.0);
         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/imm_ukf_pda.cpp

@@ -688,6 +688,10 @@ void ImmUkfPda::makeOutput(const iv::fusion::fusionobjectarray & input,
     double tv = targets_[i].x_merge_(2);
     double tyaw = targets_[i].x_merge_(3);
     double tyaw_rate = targets_[i].x_merge_(4);
+    if(tyaw_rate > 0.2)
+    {
+        tyaw_rate = 0.2;
+    }
 
     while (tyaw > M_PI)
       tyaw -= 2. * M_PI;
@@ -716,10 +720,14 @@ void ImmUkfPda::makeOutput(const iv::fusion::fusionobjectarray & input,
       if(targets_[i].object_.dimensions().x() < targets_[i].object_.dimensions().y())
       {
           iv::fusion::Dimension * pd = dd.mutable_dimensions();
-          pd->set_x(targets_[i].object_.dimensions().y());
-          pd->set_y(targets_[i].object_.dimensions().x());
+          pd->set_x(targets_[i].object_.dimensions().x());
+          pd->set_y(targets_[i].object_.dimensions().y());
 //        dd.dimensions.x = targets_[i].object_.dimensions.y;
 //        dd.dimensions.y = targets_[i].object_.dimensions.x;
+      } else {
+          iv::fusion::Dimension * pd = dd.mutable_dimensions();
+          pd->set_x(targets_[i].object_.dimensions().y());
+          pd->set_y(targets_[i].object_.dimensions().x());
       }
 
       iv::fusion::PointXYZ * pp = dd.mutable_centroid();
@@ -742,6 +750,42 @@ void ImmUkfPda::makeOutput(const iv::fusion::fusionobjectarray & input,
 
 
     updateBehaviorState(targets_[i], dd);
+    int xp ,yp;
+    if(targets_[i].object_.dimensions().x() < targets_[i].object_.dimensions().y())
+    {
+        xp = (int)((targets_[i].object_.dimensions().x()/0.2)/2.0);
+        if(xp == 0)xp=1;
+        yp = (int)((targets_[i].object_.dimensions().y()/0.2)/2.0);
+        if(yp == 0)yp=1;
+    } else {
+        xp = (int)((targets_[i].object_.dimensions().y()/0.2)/2.0);
+        if(xp == 0)xp=1;
+        yp = (int)((targets_[i].object_.dimensions().x()/0.2)/2.0);
+        if(yp == 0)yp=1;
+    }
+
+
+    int ix,iy;
+    for(ix = 0; ix<(xp*2); ix++)
+    {
+//        std::cout<<"   add  normal  --------------------------  "<<std::endl;
+        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 = 0.5;
+                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(tx + s);
+            nomal_centroid.set_nomal_y(ty + t);
+            nomal_centroid_ = dd.add_nomal_centroid();
+            nomal_centroid_->CopyFrom(nomal_centroid);
+        }
+    }
 
     if (targets_[i].is_stable_ || (targets_[i].tracking_num_ >= TrackingState::Init &&
                                    targets_[i].tracking_num_ < TrackingState::Stable))
@@ -897,6 +941,11 @@ void ImmUkfPda::tracker(const iv::fusion::fusionobjectarray & input,
   {
       std::cout<<"no obj "<<std::endl;
   }
+  for(int k =0; k<fused_objects_output.obj_size(); k++ )
+  {
+     std::cout<<"  normal size   "<< fused_objects_output.obj(k).nomal_centroid_size()<<std::endl;
+  }
+
 
   std::string out = fused_objects_output.SerializeAsString();
 //   char * strout = lidarobjtostr(lidarobjvec,ntlen);