fusion modify obs grid +/-

src/fusion/lidar_radar_fusion_cnn/fusion.hpp

@@ -395,11 +395,11 @@ void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array,
                 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(-xobs_info.xobj(j).obsang())
-                        - nomal_y*sin(-xobs_info.xobj(j).obsang());
-                float t = nomal_x*sin(-xobs_info.xobj(j).obsang())
-                        + nomal_y*cos(-xobs_info.xobj(j).obsang());
-                nomal_centroid.set_nomal_x(xobs_info.xobj(j).pos_y() + s);
+                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);

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;
@@ -149,7 +149,7 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
 
     if(li_ra_fusion.obj_size() == 0)
     {
-        std::cout<<"   fake   obj"<<std::endl;
+//        std::cout<<"   fake   obj"<<std::endl;
 
         iv::fusion::fusionobject fake_obj;
         iv::fusion::fusionobject *fake_obj_;