xunluoche commit

src/decition/common/common/roadmode_type.h

@@ -7,6 +7,7 @@ namespace iv {
     {
 
         double mfmode0 = 10;
+         double mfmode1 = 10;
         double mfmode11 = 30;
         double mfmode14 = 10;
         double mfmode15 = 10;

src/decition/common/perception_sf/impl_lidar.cpp

@@ -9,7 +9,9 @@
 #include "perceptionoutput.h"
 
 std::mutex mtx_nomal;
-
+int count = 0;
+int step = 5;
+int epoch = 2;
 
 
 void iv::perception::LiDARSensor::lidarcollect()
@@ -37,6 +39,15 @@ void iv::perception::LiDARSensor::updataFusion(std::shared_ptr<iv::fusion::fusio
     fusion_obs.swap(mfusion_obs_);
     mMutex.unlock();
 
+    ++count ;
+    if(count/epoch ==0 )
+    {
+        count = 0;
+
+
+    }
+
+
 
 
     fusion_ptr_ = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));

src/decition/decition_brain_sf/decition/adc_controller/pid_controller.cpp

@@ -198,7 +198,7 @@ float iv::decition::PIDController::getPidAcc (GPS_INS gpsIns,float dSpeed, float
         ttc = obsDistance / (vh - vl);
 
 
-    ServiceCarStatus.mfttc = ttc;
+ //   ServiceCarStatus.mfttc = ttc;
 
 
     if (obsDistance <= dmax)

src/decition/decition_brain_sf/decition/brain.cpp

@@ -249,6 +249,9 @@ void iv::decition::BrainDecition::run() {
     std::string strroadmode_vel = xp.GetParam("roadmode0","10");
     ServiceCarStatus.mroadmode_vel.mfmode0 = atof(strroadmode_vel.data());
 
+     strroadmode_vel= xp.GetParam("roadmode1","10");
+    ServiceCarStatus.mroadmode_vel.mfmode1 = atof(strroadmode_vel.data());
+
     strroadmode_vel = xp.GetParam("roadmode11","30");
     ServiceCarStatus.mroadmode_vel.mfmode11 = atof(strroadmode_vel.data());
 

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -1287,7 +1287,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }*/
     }else if (gpsMapLine[PathPoint]->roadMode == 1)
     {
-        dSpeed = min(10.0,dSpeed);
+        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode1,dSpeed);
     }else if (gpsMapLine[PathPoint]->roadMode == 2)
     {
         dSpeed = min(15.0,dSpeed);
@@ -2515,7 +2515,7 @@ void iv::decition::DecideGps00::computeObsOnRoad(iv::LidarGridPtr lidarGridPtr,
     obsSd= obsPoint.obs_speed_y;
 
     if(ServiceCarStatus.useLidarPerPredict){
-       double preDis= predictObsOnRoad(lidar_per, gpsTrace, realspeed);
+       double preDis= predictObsOnRoad(lidar_per, gpsTrace, 1.0);
        if (preDis<obs){
            obs = preDis;
            if(abs(obs-preDis>0.5)){
@@ -2665,7 +2665,7 @@ double iv::decition::DecideGps00::predictObsOnRoad(std::vector<iv::Perception::P
         preObsDis=lastPreObsDistance;
     }
 
-  //  ServiceCarStatus.mfttc = preObsDis;
+    ServiceCarStatus.mfttc = preObsDis;
     return preObsDis;
 //    if(preObsDis<obsDistance){
 //        obsDistance=preObsDis;

src/fusion/lidar_radar_fusion/fusion.hpp

@@ -5,10 +5,12 @@
 #include <iostream>
 #include "opencv2/opencv.hpp"
 #include "perceptionoutput.h"
+#include "Eigen/Eigen"
 
 namespace iv {
 namespace fusion {
 
+//std::vector<Match_index> match_idxs;
 
 static float time_step = 0.3;
 static std::vector<std::string> labels = {"unknown","ped","bike","car","bus_or_truck"};
@@ -19,301 +21,329 @@ float ComputeDis(cv::Point2f po1, cv::Point2f po2)
     return (sqrt(pow((po1.x-po2.x),2) + pow((po1.y-po2.y),2)));
 }
 
-void RLfusion(LidarObject& lidar_object_vector,iv::radar::radarobjectarray& radar_object_array, iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
+void Get_AssociationMat(LidarObject& lidar_object_vector,iv::radar::radarobjectarray& radar_object_array,
+                        std::vector<match_index>& match_idx,std::vector<int>&radar_idx)
 {
-    lidar_radar_fusion_object_array.Clear();
-//    std::cout<<"~~~~~~~~~~~~"<<lidar_object_array.obj_size()<<std::endl;
-
+//    std::cout<<" is      ok     "<<std::endl;
+    int nlidar = lidar_object_vector.size();
+    int nradar = radar_object_array.obj_size();
+    match_idx.clear();
+    radar_idx.clear();
     for(int i = 0; i<lidar_object_vector.size(); i++)
     {
+        match_index match;
+        match.nlidar = i;
         std::vector<int> fuindex;
+//        std::cout<<"  size    "<<radar_object_array.obj_size()<<std::endl;
         for(int j =0; j<radar_object_array.obj_size(); j++)
         {
+//            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.at(i))))
             {
                 fuindex.push_back(j);
             }
         }
-        if(fuindex.size() == 0)
+        if(fuindex.size() >0)
+        {
+            std::vector<float> dis;
+            cv::Point2f po1;
+            po1.x = lidar_object_vector.at(i).location.x;
+            po1.y = lidar_object_vector.at(i).location.y;
+            for(std::vector<int>::iterator d = fuindex.begin(); d !=fuindex.end(); d++)
+            {
+                cv::Point2f po2;
+                po2.x = radar_object_array.obj(*d).x();
+                po2.y = radar_object_array.obj(*d).y();
+                dis.push_back(ComputeDis(po1,po2));
+            }
+            auto smallest = std::min_element(std::begin(dis), std::end(dis));
+            int index = std::distance(std::begin(dis), smallest);
+            dis.clear();
+            match.nradar = index;
+        }else {
+            match.nradar = -1000;
+        }
+//        std::cout<<" fuindex     nradar  "<<fuindex.size()<<"    "<<match.nradar<<std::endl;
+
+        match_idx.push_back(match);
+    }
+
+    for(int k = 0; k<radar_object_array.obj_size(); k++)
+    {
+        std::vector<int> index;
+        for(std::vector<match_index>::iterator l = match_idx.begin(); l != match_idx.end(); l++)
+        {
+            index.push_back(l->nradar);
+        }
+        if(std::find(index.begin(),index.end(),k) !=index.end())
+        {
+            radar_idx.push_back(k);
+        }
+    }
+}
+
+void RLfusion(LidarObject& lidar_object_vector,iv::radar::radarobjectarray& radar_object_array, iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
+{
+    lidar_radar_fusion_object_array.Clear();
+    std::vector<match_index> match_idx;
+    std::vector<int> radar_idx;
+    Get_AssociationMat(lidar_object_vector,radar_object_array,match_idx,radar_idx);
+    for(int i =0; i< match_idx.size(); i++)
+    {
+        iv::fusion::fusionobject fusion_object;
+        fusion_object.set_id(lidar_object_vector.at(match_idx.at(i).nlidar).tracker_id);
+        fusion_object.set_type_name(labels[lidar_object_vector.at(match_idx.at(i).nlidar).label]);
+        fusion_object.set_prob(lidar_object_vector.at(match_idx.at(i).nlidar).robustness);
+        if(match_idx.at(i).nradar == -1000)
         {
-            iv::fusion::fusionobject fusion_object;
-            fusion_object.set_id(lidar_object_vector.at(i).tracker_id);
-            fusion_object.set_type_name(labels[lidar_object_vector.at(i).label]);
-            fusion_object.set_prob(lidar_object_vector.at(i).robustness);
-            fusion_object.set_yaw(lidar_object_vector.at(i).yaw);
-            fusion_object.set_lifetime(lidar_object_vector.at(i).visible_life);
+//            std::cout<<"  lidar    ok  "<<std::endl;
+            fusion_object.set_yaw(lidar_object_vector.at(match_idx.at(i).nlidar).yaw);
+            fusion_object.set_lifetime(lidar_object_vector.at(match_idx.at(i).nlidar).visible_life);
 
             iv::fusion::VelXY vel_relative;
             iv::fusion::VelXY *vel_relative_;
-            vel_relative.set_x(lidar_object_vector.at(i).velocity.x);
-            vel_relative.set_y(lidar_object_vector.at(i).velocity.y);
+            vel_relative.set_x(lidar_object_vector.at(match_idx.at(i).nlidar).velocity.x);
+            vel_relative.set_y(lidar_object_vector.at(match_idx.at(i).nlidar).velocity.y);
             vel_relative_ = fusion_object.mutable_vel_relative();
             vel_relative_->CopyFrom(vel_relative);
 
             iv::fusion::VelXY vel_abs;
             iv::fusion::VelXY *vel_abs_;
-            vel_abs.set_x(lidar_object_vector.at(i).velocity_abs.x);
-            vel_abs.set_y(lidar_object_vector.at(i).velocity_abs.y);
+            vel_abs.set_x(lidar_object_vector.at(match_idx.at(i).nlidar).velocity_abs.x);
+            vel_abs.set_y(lidar_object_vector.at(match_idx.at(i).nlidar).velocity_abs.y);
             vel_abs_ = fusion_object.mutable_vel_abs();
             vel_abs_->CopyFrom(vel_abs);
 
-            iv::fusion::AccXY acc_relative;
-            iv::fusion::AccXY *acc_relative_;
-            acc_relative.set_x(lidar_object_vector.at(i).acceleration_abs.x);
-            acc_relative.set_y(lidar_object_vector.at(i).acceleration_abs.y);
-            acc_relative_ = fusion_object.mutable_acc_relative();
-            acc_relative_->CopyFrom(acc_relative);
-
-            iv::fusion::PointXYZ min_point;
-            iv::fusion::PointXYZ *min_point_;
-            min_point.set_x(lidar_object_vector.at(i).nearest_point.x);
-            min_point.set_y(lidar_object_vector.at(i).nearest_point.y);
-            min_point_ = fusion_object.mutable_min_point();
-            min_point_->CopyFrom(min_point);
-
             iv::fusion::PointXYZ centroid;
             iv::fusion::PointXYZ *centroid_;
-            centroid.set_x(lidar_object_vector.at(i).location.x);
-            centroid.set_y(lidar_object_vector.at(i).location.y);
-            centroid.set_z(lidar_object_vector.at(i).location.z);
+            centroid.set_x(lidar_object_vector.at(match_idx.at(i).nlidar).location.x);
+            centroid.set_y(lidar_object_vector.at(match_idx.at(i).nlidar).location.y);
+            centroid.set_z(lidar_object_vector.at(match_idx.at(i).nlidar).location.z);
             centroid_ = fusion_object.mutable_centroid();
             centroid_->CopyFrom(centroid);
+        }else {
+//            std::cout<<"  radar                           ok  "<<std::endl;
+            fusion_object.set_yaw(radar_object_array.obj(match_idx.at(i).nradar).angle());
+            fusion_object.set_lifetime(radar_object_array.obj(match_idx.at(i).nradar).has_bridge_object());
 
-            iv::fusion::Dimension dimension;
-            iv::fusion::Dimension *dimension_;
-            dimension.set_x(lidar_object_vector.at(i).size.x);
-            dimension.set_y(lidar_object_vector.at(i).size.y);
-            dimension.set_z(lidar_object_vector.at(i).size.z);
-            dimension_ = fusion_object.mutable_dimensions();
-            dimension_->CopyFrom(dimension);
+            iv::fusion::VelXY vel_relative;
+            iv::fusion::VelXY *vel_relative_;
+            vel_relative.set_x(radar_object_array.obj(match_idx.at(i).nradar).vx());
+            vel_relative.set_y(radar_object_array.obj(match_idx.at(i).nradar).vy());
+            vel_relative_ = fusion_object.mutable_vel_relative();
+            vel_relative_->CopyFrom(vel_relative);
+
+            iv::fusion::VelXY vel_abs;
+            iv::fusion::VelXY *vel_abs_;
+            vel_abs.set_x(radar_object_array.obj(match_idx.at(i).nradar).vx());
+            vel_abs.set_y(radar_object_array.obj(match_idx.at(i).nradar).vy());
+            vel_abs_ = fusion_object.mutable_vel_abs();
+            vel_abs_->CopyFrom(vel_abs);
 
-            if((lidar_object_vector.at(i).size.x>0)&&(lidar_object_vector.at(i).size.y>0))
+            iv::fusion::PointXYZ centroid;
+            iv::fusion::PointXYZ *centroid_;
+            centroid.set_x(lidar_object_vector.at(match_idx.at(i).nradar).location.x);
+            centroid.set_y(lidar_object_vector.at(match_idx.at(i).nradar).location.y);
+            centroid.set_z(lidar_object_vector.at(match_idx.at(i).nradar).location.z);
+            centroid_ = fusion_object.mutable_centroid();
+            centroid_->CopyFrom(centroid);
+        }
+        iv::fusion::AccXY acc_relative;
+        iv::fusion::AccXY *acc_relative_;
+        acc_relative.set_x(lidar_object_vector.at(match_idx.at(i).nlidar).acceleration_abs.x);
+        acc_relative.set_y(lidar_object_vector.at(match_idx.at(i).nlidar).acceleration_abs.y);
+        acc_relative_ = fusion_object.mutable_acc_relative();
+        acc_relative_->CopyFrom(acc_relative);
+
+        iv::fusion::PointXYZ min_point;
+        iv::fusion::PointXYZ *min_point_;
+        min_point.set_x(lidar_object_vector.at(match_idx.at(i).nlidar).nearest_point.x);
+        min_point.set_y(lidar_object_vector.at(match_idx.at(i).nlidar).nearest_point.y);
+        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_vector.at(match_idx.at(i).nlidar).size.x);
+        dimension.set_y(lidar_object_vector.at(match_idx.at(i).nlidar).size.y);
+        dimension.set_z(lidar_object_vector.at(match_idx.at(i).nlidar).size.z);
+        dimension_ = fusion_object.mutable_dimensions();
+        dimension_->CopyFrom(dimension);
+
+        if((lidar_object_vector.at(match_idx.at(i).nlidar).size.x>0)&&(lidar_object_vector.at(match_idx.at(i).nlidar).size.y>0))
+        {
+            int xp = (int)((lidar_object_vector.at(match_idx.at(i).nlidar).size.x/0.2)/2.0);
+            if(xp == 0)xp=1;
+            int yp = (int)((lidar_object_vector.at(match_idx.at(i).nlidar).size.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_vector.at(match_idx.at(i).nlidar).location.z;
+                    float s = nomal_x*cos(lidar_object_vector.at(match_idx.at(i).nlidar).yaw) - nomal_y*sin(lidar_object_vector.at(match_idx.at(i).nlidar).yaw);
+                    float t = nomal_x*sin(lidar_object_vector.at(match_idx.at(i).nlidar).yaw) + nomal_y*cos(lidar_object_vector.at(i).yaw);
+                    nomal_centroid.set_nomal_x(lidar_object_vector.at(match_idx.at(i).nlidar).location.x + s);
+                    nomal_centroid.set_nomal_y(lidar_object_vector.at(match_idx.at(i).nlidar).location.y+ t);
+                    nomal_centroid_ = fusion_object.add_nomal_centroid();
+                    nomal_centroid_->CopyFrom(nomal_centroid);
+                }
+            }
+        }
+        for(int k = 0; k<10; k++)
+        {
+//            std::cout<<"fusion    begin"<<std::endl;
+            iv::fusion::PointXYZ point_forcaste;
+            iv::fusion::PointXYZ *point_forcaste_;
+            float forcast_x = lidar_object_vector.at(match_idx.at(i).nlidar).location.x + lidar_object_vector.at(match_idx.at(i).nlidar).velocity.x*time_step*(k+1);
+            float forcast_y = lidar_object_vector.at(match_idx.at(i).nlidar).location.y + lidar_object_vector.at(match_idx.at(i).nlidar).velocity.y*time_step*(k+1);
+            point_forcaste.set_x(forcast_x);
+            point_forcaste.set_y(forcast_y);
+            point_forcaste.set_z(lidar_object_vector.at(match_idx.at(i).nlidar).location.z);
+            point_forcaste_ = fusion_object.add_point_forecast();
+            point_forcaste_->CopyFrom(point_forcaste);
+
+            iv::fusion::NomalForecast forcast_normal;
+            iv::fusion::NomalForecast *forcast_normal_;
+            forcast_normal.set_forecast_index(i);
+//            std::cout<<"fusion     end"<<std::endl;
+            if((lidar_object_vector.at(i).size.x>0)&&(lidar_object_vector.at(match_idx.at(i).nlidar).size.y>0))
             {
-                int xp = (int)((lidar_object_vector.at(i).size.x/0.2)/2.0);
+                int xp = (int)((lidar_object_vector.at(match_idx.at(i).nlidar).size.x/0.2)/2.0);
                 if(xp == 0)xp=1;
-                int yp = (int)((lidar_object_vector.at(i).size.y/0.2)/2.0);
+                int yp = (int)((lidar_object_vector.at(match_idx.at(i).nlidar).size.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_;
+                        iv::fusion::NomalXYZ nomal_forcast;
+                        iv::fusion::NomalXYZ *nomal_forcast_;
                         float nomal_x = ix*0.2 - xp*0.2;
                         float nomal_y = iy*0.2 - yp*0.2;
-                        float nomal_z = lidar_object_vector.at(i).location.z;
-                        float s = nomal_x*cos(lidar_object_vector.at(i).yaw) - nomal_y*sin(lidar_object_vector.at(i).yaw);
-                        float t = nomal_x*sin(lidar_object_vector.at(i).yaw) + nomal_y*cos(lidar_object_vector.at(i).yaw);
-                        nomal_centroid.set_nomal_x(lidar_object_vector.at(i).location.x + s);
-                        nomal_centroid.set_nomal_y(lidar_object_vector.at(i).location.y+ t);
-                        nomal_centroid_ = fusion_object.add_nomal_centroid();
-                        nomal_centroid_->CopyFrom(nomal_centroid);
+                        float nomal_z = lidar_object_vector.at(match_idx.at(i).nlidar).location.z;
+                        float s = nomal_x*cos(lidar_object_vector.at(match_idx.at(i).nlidar).yaw) - nomal_y*sin(lidar_object_vector.at(match_idx.at(i).nlidar).yaw);
+                        float t = nomal_x*sin(lidar_object_vector.at(match_idx.at(i).nlidar).yaw) + nomal_y*cos(lidar_object_vector.at(match_idx.at(i).nlidar).yaw);
+                        nomal_forcast.set_nomal_x(forcast_x + s);
+                        nomal_forcast.set_nomal_y(forcast_y + t);
+                        nomal_forcast_ = forcast_normal.add_forecast_nomal();
+                        nomal_forcast_->CopyFrom(nomal_forcast);
                     }
                 }
-
             }
+            forcast_normal_=fusion_object.add_forecast_nomals();
+            forcast_normal_->CopyFrom(forcast_normal);
+        }
 
-            for(int k = 0; k<10; k++)
-            {
-                std::cout<<"fusion    begin"<<std::endl;
-                iv::fusion::PointXYZ point_forcaste;
-                iv::fusion::PointXYZ *point_forcaste_;
-                float forcast_x = lidar_object_vector.at(i).location.x + lidar_object_vector.at(i).velocity.x*time_step*(k+1);
-                float forcast_y = lidar_object_vector.at(i).location.y + lidar_object_vector.at(i).velocity.y*time_step*(k+1);
-                point_forcaste.set_x(forcast_x);
-                point_forcaste.set_y(forcast_y);
-                point_forcaste.set_z(lidar_object_vector.at(i).location.z);
-                point_forcaste_ = fusion_object.add_point_forecast();
-                point_forcaste_->CopyFrom(point_forcaste);
-
-                iv::fusion::NomalForecast forcast_normal;
-                iv::fusion::NomalForecast *forcast_normal_;
-                forcast_normal.set_forecast_index(i);
-                                            std::cout<<"fusion     end"<<std::endl;
-                if((lidar_object_vector.at(i).size.x>0)&&(lidar_object_vector.at(i).size.y>0))
-                {
-                    int xp = (int)((lidar_object_vector.at(i).size.x/0.2)/2.0);
-                    if(xp == 0)xp=1;
-                    int yp = (int)((lidar_object_vector.at(i).size.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_forcast;
-                            iv::fusion::NomalXYZ *nomal_forcast_;
-                            float nomal_x = ix*0.2 - xp*0.2;
-                            float nomal_y = iy*0.2 - yp*0.2;
-                            float nomal_z = lidar_object_vector.at(i).location.z;
-                            float s = nomal_x*cos(lidar_object_vector.at(i).yaw) - nomal_y*sin(lidar_object_vector.at(i).yaw);
-                            float t = nomal_x*sin(lidar_object_vector.at(i).yaw) + nomal_y*cos(lidar_object_vector.at(i).yaw);
-                            nomal_forcast.set_nomal_x(forcast_x + s);
-                            nomal_forcast.set_nomal_y(forcast_y + t);
-                            nomal_forcast_ = forcast_normal.add_forecast_nomal();
-                            nomal_forcast_->CopyFrom(nomal_forcast);
-                        }
-                    }
-                }
-                forcast_normal_=fusion_object.add_forecast_nomals();
-                forcast_normal_->CopyFrom(forcast_normal);
-            }
-
-            iv::fusion::fusionobject *pobj = lidar_radar_fusion_object_array.add_obj();
-            pobj->CopyFrom(fusion_object);
-         }else
-         {
-            std::vector<float> dis;
-            cv::Point2f po1;
-            po1.x = lidar_object_vector.at(i).location.x;
-            po1.y = lidar_object_vector.at(i).location.y;
-            for(std::vector<int>::iterator d = fuindex.begin(); d !=fuindex.end(); d++)
-            {
-                cv::Point2f po2;
-                po2.x = radar_object_array.obj(*d).x();
-                po2.y = radar_object_array.obj(*d).y();
-                dis.push_back(ComputeDis(po1,po2));
-            }
-            auto smallest = std::min_element(std::begin(dis), std::end(dis));
-            int index = std::distance(std::begin(dis), smallest);
-            dis.clear();
-
-            iv::fusion::fusionobject fusion_object;
-            fusion_object.set_id(lidar_object_vector.at(i).tracker_id);
-            fusion_object.set_type_name(labels[lidar_object_vector.at(i).label]);
-            fusion_object.set_prob(lidar_object_vector.at(i).robustness);
-            fusion_object.set_yaw(lidar_object_vector.at(i).yaw);
-            fusion_object.set_lifetime(lidar_object_vector.at(i).visible_life);
+        iv::fusion::fusionobject *pobj = lidar_radar_fusion_object_array.add_obj();
+        pobj->CopyFrom(fusion_object);
+    }
+        for(int j = 0; j< radar_idx.size() ; j++){
+            if(radar_object_array.obj(j).bvalid() == false) continue;
+            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());
             iv::fusion::VelXY vel_relative;
             iv::fusion::VelXY *vel_relative_;
-            vel_relative.set_x(radar_object_array.obj(fuindex[index]).vx());
-            vel_relative.set_y(radar_object_array.obj(fuindex[index]).vy());
-            vel_relative_ = fusion_object.mutable_vel_relative();
+            vel_relative.set_x(radar_object_array.obj(j).vx());
+            vel_relative.set_y(radar_object_array.obj(j).vy());
+            vel_relative_ = fusion_obj.mutable_vel_relative();
             vel_relative_->CopyFrom(vel_relative);
 
-            iv::fusion::VelXY vel_abs;
-            iv::fusion::VelXY *vel_abs_;
-            vel_abs.set_x(radar_object_array.obj(fuindex[index]).vx());
-            vel_abs.set_y(radar_object_array.obj(fuindex[index]).vy());
-            vel_abs_ = fusion_object.mutable_vel_abs();
-            vel_abs_->CopyFrom(vel_abs);
-
-
-            iv::fusion::AccXY acc_relative;
-            iv::fusion::AccXY *acc_relative_;
-            acc_relative.set_x(radar_object_array.obj(fuindex[index]).range_accel()*
-                               cos(radar_object_array.obj(fuindex[index]).angle()));
-            acc_relative.set_y(radar_object_array.obj(fuindex[index]).range_accel()*
-                               sin(radar_object_array.obj(fuindex[index]).angle()));
-            acc_relative_ = fusion_object.mutable_acc_relative();
-            acc_relative_->CopyFrom(acc_relative);
-
             iv::fusion::PointXYZ centroid;
             iv::fusion::PointXYZ *centroid_;
-            centroid.set_x(radar_object_array.obj(fuindex[index]).x());
-            centroid.set_y(radar_object_array.obj(fuindex[index]).y());
-            centroid.set_z(lidar_object_vector.at(i).location.z);
-            centroid_ = fusion_object.mutable_centroid();
+            centroid.set_x(radar_object_array.obj(j).x());
+            centroid.set_y(radar_object_array.obj(j).y());
+            centroid.set_z(1.0);
+            centroid_ = fusion_obj.mutable_centroid();
             centroid_->CopyFrom(centroid);
 
-            iv::fusion::PointXYZ min_point;
-            iv::fusion::PointXYZ *min_point_;
-            min_point.set_x(lidar_object_vector.at(i).nearest_point.x);
-            min_point.set_y(lidar_object_vector.at(i).nearest_point.y);
-            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_vector.at(i).size.x);
-            dimension.set_y(lidar_object_vector.at(i).size.y);
-            dimension.set_z(lidar_object_vector.at(i).size.z);
-            dimension_ = fusion_object.mutable_dimensions();
+            dimension.set_x(0.5);
+            dimension.set_y(0.5);
+            dimension.set_z(1.0);
+            dimension_ = fusion_obj.mutable_dimensions();
             dimension_->CopyFrom(dimension);
 
-
-            if((lidar_object_vector.at(i).size.x>0)&&(lidar_object_vector.at(i).size.y>0))
+            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++)
             {
-                int xp = (int)((lidar_object_vector.at(i).size.x/0.2)/2.0);
-                if(xp == 0)xp=1;
-                int yp = (int)((lidar_object_vector.at(i).size.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++)
                 {
-                    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_vector.at(i).location.z;
-                        float s = nomal_x*cos(lidar_object_vector.at(i).yaw) - nomal_y*sin(lidar_object_vector.at(i).yaw);
-                        float t = nomal_x*sin(lidar_object_vector.at(i).yaw) + nomal_y*cos(lidar_object_vector.at(i).yaw);
-                        nomal_centroid.set_nomal_x(radar_object_array.obj(fuindex[index]).x() + s);
-                        nomal_centroid.set_nomal_y(radar_object_array.obj(fuindex[index]).y() + t);
-                        nomal_centroid_ = fusion_object.add_nomal_centroid();
-                        nomal_centroid_->CopyFrom(nomal_centroid);
-                    }
+                    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(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);
+                    nomal_centroid_ = fusion_obj.add_nomal_centroid();
+                    nomal_centroid_->CopyFrom(nomal_centroid);
                 }
-
             }
+
             for(int k = 0; k<10; k++)
             {
+//                std::cout<<"fusion    begin"<<std::endl;
                 iv::fusion::PointXYZ point_forcaste;
                 iv::fusion::PointXYZ *point_forcaste_;
-                float forcast_x = radar_object_array.obj(fuindex[index]).x() + radar_object_array.obj(fuindex[index]).vx()*time_step*(k+1);
-                float forcast_y = radar_object_array.obj(fuindex[index]).y() + radar_object_array.obj(fuindex[index]).vy()*time_step*(k+1);
+                float forcast_x = radar_object_array.obj(j).x() + radar_object_array.obj(j).vx()*time_step*(k+1);
+                float forcast_y = radar_object_array.obj(j).y() + radar_object_array.obj(j).vy()*time_step*(k+1);
                 point_forcaste.set_x(forcast_x);
                 point_forcaste.set_y(forcast_y);
-                point_forcaste.set_z(lidar_object_vector.at(i).location.z);
-                point_forcaste_ = fusion_object.add_point_forecast();
+                point_forcaste.set_z(1.0);
+                point_forcaste_ = fusion_obj.add_point_forecast();
                 point_forcaste_->CopyFrom(point_forcaste);
 
                 iv::fusion::NomalForecast forcast_normal;
                 iv::fusion::NomalForecast *forcast_normal_;
-                forcast_normal.set_forecast_index(i);
-                if((lidar_object_vector.at(i).size.x>0)&&(lidar_object_vector.at(i).size.y>0))
+                forcast_normal.set_forecast_index(j);
+
+                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++)
                 {
-                    int xp = (int)((lidar_object_vector.at(i).size.x/0.2)/2.0);
-                    if(xp == 0)xp=1;
-                    int yp = (int)((lidar_object_vector.at(i).size.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++)
                     {
-                        for(iy = 0; iy<(yp*2); iy++)
-                        {
-                            iv::fusion::NomalXYZ nomal_forcast;
-                            iv::fusion::NomalXYZ *nomal_forcast_;
-                            float nomal_x = ix*0.2 - xp*0.2;
-                            float nomal_y = iy*0.2 - yp*0.2;
-                            float nomal_z = lidar_object_vector.at(i).location.z;
-                            float s = nomal_x*cos(lidar_object_vector.at(i).yaw) - nomal_y*sin(lidar_object_vector.at(i).yaw);
-                            float t = nomal_x*sin(lidar_object_vector.at(i).yaw) + nomal_y*cos(lidar_object_vector.at(i).yaw);
-                            nomal_forcast.set_nomal_x(forcast_x + s);
-                            nomal_forcast.set_nomal_y(forcast_y + t);
-                            nomal_forcast_ = forcast_normal.add_forecast_nomal();
-                            nomal_forcast_->CopyFrom(nomal_forcast);
-                        }
+                        iv::fusion::NomalXYZ nomal_forcast;
+                        iv::fusion::NomalXYZ *nomal_forcast_;
+                        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_forcast.set_nomal_x(forcast_x + s);
+                        nomal_forcast.set_nomal_y(forcast_y + t);
+                        nomal_forcast_ = forcast_normal.add_forecast_nomal();
+                        nomal_forcast_->CopyFrom(nomal_forcast);
                     }
                 }
-                forcast_normal_=fusion_object.add_forecast_nomals();
+                forcast_normal_=fusion_obj.add_forecast_nomals();
                 forcast_normal_->CopyFrom(forcast_normal);
             }
 
-            iv::fusion::fusionobject *pobj = lidar_radar_fusion_object_array.add_obj();
-
-            pobj->CopyFrom(fusion_object);
-        }
-        fuindex.clear();
-    }
+            iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
+            obj->CopyFrom(fusion_obj);
+        }  
 }
-
 }
 }
 

src/fusion/lidar_radar_fusion/fusion_probabilities.cpp

@@ -1,22 +1,38 @@
 #include "fusion_probabilities.h"
 
 //毫米波雷达object点是否和激光雷达object的俯视box匹配
+//int iv::fusion::FusionProbabilities::ComputRadarLidarmatch(const  iv::radar::radarobject& radarPoint, const  iv::Perception::PerceptionOutput& lidarobject)
+//{
+//    Eigen::Matrix<float,3,1> radar_in_radar, radar_in_lidar;
+//    radar_in_radar << radarPoint.x(), radarPoint.y(),-0.95;
+//    radar_in_lidar = iv::fusion::Transformation::RadarToLidar(radar_in_radar);
+//    if(!((radar_in_lidar[0]>=(lidarobject.nearest_point.x))&&(radar_in_lidar[1]>= (lidarobject.nearest_point.y))&&
+//         (radar_in_lidar[0]<=(lidarobject.location.x + lidarobject.size.x*0.5))&&(radar_in_lidar[1]<=(lidarobject.location.y + lidarobject.size.y * 0.5))))
+//    {
+//        return 0;
+//    } else {
+//        return 1;
+//    }
+//}
+
+
 int iv::fusion::FusionProbabilities::ComputRadarLidarmatch(const  iv::radar::radarobject& radarPoint, const  iv::Perception::PerceptionOutput& lidarobject)
 {
-    Eigen::Matrix<float,3,1> radar_in_radar, radar_in_lidar;
-    radar_in_radar << radarPoint.x(), radarPoint.y(),-0.95;
-    radar_in_lidar = iv::fusion::Transformation::RadarToLidar(radar_in_radar);
-    if(!((radar_in_lidar[0]>=(lidarobject.nearest_point.x))&&(radar_in_lidar[1]>= (lidarobject.nearest_point.y))&&
-         (radar_in_lidar[0]<=(lidarobject.location.x + lidarobject.size.x*0.5))&&(radar_in_lidar[1]<=(lidarobject.location.y + lidarobject.size.y * 0.5))))
+//    std::cout<<"  lidar   x   y   "<<lidarobject.nearest_point.x<<"           "<<lidarobject.nearest_point.y<<std::endl;
+//    std::cout<<"  radar   x   y   "<<radarPoint.x()<<"     "<<radarPoint.y()<<std::endl;
+    if(!((radarPoint.x()  >=(lidarobject.nearest_point.x))&&( radarPoint.y() >= (lidarobject.nearest_point.y))&&
+         (radarPoint.x()  <=(lidarobject.location.x + lidarobject.size.x*0.5))&&(radarPoint.y()<=(lidarobject.location.y + lidarobject.size.y * 0.5 + 1))))
     {
         return 0;
     } else {
         return 1;
     }
-}
 
 
 
+}
+
+
 
 
 

src/fusion/lidar_radar_fusion/fusion_probabilities.h

@@ -9,6 +9,7 @@
 //    Point min_point;
 //};
 
+
 namespace iv {
 namespace fusion {
 

src/fusion/lidar_radar_fusion/main.cpp

@@ -89,10 +89,10 @@ void datafusion()
 //    gMutex.lock();
 //    Transformation::RadarPross(radarobjvec);
     RLfusion(lidar_obj,radarobjvec,li_ra_fusion);
-    std::cout<<li_ra_fusion.obj_size()<<std::endl;
+//    std::cout<<li_ra_fusion.obj_size()<<std::endl;
 
 //    gMutex.lock();
-    std::cout<<" track   begin"<<std::endl;
+//    std::cout<<" track   begin"<<std::endl;
     //---------------------------------------------  init tracker  -------------------------------------------------
     if (!m_isTrackerInitialized)
     {
@@ -103,7 +103,7 @@ void datafusion()
         }
     }
     Tracking(li_ra_fusion, tracker);
-    std::cout<<"track    end"<<std::endl;
+//    std::cout<<"track    end"<<std::endl;
 
     //--------------------------------------------  end tracking  ---------------------------------------------------
 //    gMutex.unlock();
@@ -139,7 +139,7 @@ int main(int argc, char *argv[])
     tracker.setSettings(settings);
 
     void *gpa;
-    gpa = iv::modulecomm::RegisterRecv("radar_esr_f",Listenesrfront);
+    gpa = iv::modulecomm::RegisterRecv("radar",Listenesrfront);
 //    gpa = iv::modulecomm::RegisterRecv("lidar_track",Listenlidarcnndetect);
     gpa = iv::modulecomm::RegisterRecv("lidar_per",ListenPer);
     return a.exec();

src/fusion/lidar_radar_fusion/transformation.h

@@ -20,6 +20,11 @@ struct Vel_Struct{
     double vy;
 };
 
+struct match_index
+{
+    int nlidar;
+    int nradar;
+};
 
 namespace iv {
 namespace fusion {