sensor

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -1615,6 +1615,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
     }
 
+
     std::cout<<"\n原始RoadOri:%d\n"<<roadOri<<endl;
     //   计算回归原始路线
 #ifdef AVOID_NEW

src/fusion/lidar_radar_fusion/fusion.hpp

@@ -41,8 +41,9 @@ void Get_AssociationMat(LidarObjectPtr& lidar_object_vector_ptr,iv::radar::radar
         {
 //            std::cout<<"  is     ok   "<<std::endl;
             if(radar_object_array.obj(j).bvalid() == false) continue;
-            if(!(iv::fusion::FusionProbabilities::ComputRadarLidarmatch(radar_object_array.obj(j),lidar_object_vector_ptr->at(i))))
+            if(iv::fusion::FusionProbabilities::ComputRadarLidarmatch(radar_object_array.obj(j),lidar_object_vector_ptr->at(i)))
             {
+//                std::cout<<"   matched      j"<<j<<std::endl;
                 fuindex.push_back(j);
             }
         }
@@ -212,18 +213,18 @@ void RLfusion(LidarObjectPtr& lidar_object_vector_ptr,iv::radar::radarobjectarra
             if(abs(radar_object_array.obj(j).x())<4 && radar_object_array.obj(j).y()< 100) continue;
 
             iv::fusion::fusionobject fusion_obj;
-            fusion_obj.set_yaw(radar_object_array.obj(j).angle());
+            fusion_obj.set_yaw(radar_object_array.obj(radar_idx.at(j)).angle());
             iv::fusion::VelXY vel_relative;
             iv::fusion::VelXY *vel_relative_;
-            vel_relative.set_x(radar_object_array.obj(j).vx());
-            vel_relative.set_y(radar_object_array.obj(j).vy());
+            vel_relative.set_x(radar_object_array.obj(radar_idx.at(j)).vx());
+            vel_relative.set_y(radar_object_array.obj(radar_idx.at(j)).vy());
             vel_relative_ = fusion_obj.mutable_vel_relative();
             vel_relative_->CopyFrom(vel_relative);
 
             iv::fusion::PointXYZ centroid;
             iv::fusion::PointXYZ *centroid_;
-            centroid.set_x(radar_object_array.obj(j).x());
-            centroid.set_y(radar_object_array.obj(j).y());
+            centroid.set_x(radar_object_array.obj(radar_idx.at(j)).x());
+            centroid.set_y(radar_object_array.obj(radar_idx.at(j)).y());
             centroid.set_z(1.0);
             centroid_ = fusion_obj.mutable_centroid();
             centroid_->CopyFrom(centroid);
@@ -250,10 +251,10 @@ void RLfusion(LidarObjectPtr& lidar_object_vector_ptr,iv::radar::radarobjectarra
                     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(j).angle()) - nomal_y*sin(radar_object_array.obj(j).angle());
-                    float t = nomal_x*sin(radar_object_array.obj(j).angle()) + nomal_y*cos(radar_object_array.obj(j).angle());
-                    nomal_centroid.set_nomal_x(radar_object_array.obj(j).x() + s);
-                    nomal_centroid.set_nomal_y(radar_object_array.obj(j).y() + t);
+                    float s = nomal_x*cos(radar_object_array.obj(radar_idx.at(j)).angle()) - nomal_y*sin(radar_object_array.obj(radar_idx.at(j)).angle());
+                    float t = nomal_x*sin(radar_object_array.obj(radar_idx.at(j)).angle()) + nomal_y*cos(radar_object_array.obj(radar_idx.at(j)).angle());
+                    nomal_centroid.set_nomal_x(radar_object_array.obj(radar_idx.at(j)).x() + s);
+                    nomal_centroid.set_nomal_y(radar_object_array.obj(radar_idx.at(j)).y() + t);
                     nomal_centroid_ = fusion_obj.add_nomal_centroid();
                     nomal_centroid_->CopyFrom(nomal_centroid);
                 }