guan guang che

src/decition/common/perception_sf/eyes.cpp

@@ -1,5 +1,9 @@
 #include <perception_sf/eyes.h>
 #include <time.h>
+#include <QTime>
+
+#include "ivlog.h"
+extern iv::Ivlog * givlog;
 
 iv::perception::Eyes::Eyes() {
     mgpsindex = 0;
@@ -125,6 +129,13 @@ void iv::perception::Eyes::getSensorObstacle(iv::ObsRadar& brain_obs_radar, iv::
                 *data = *gps_ins_data_;
                 brain_gps_data =  data;
                 mgpsreadindex = mgpsindex;
+                QTime current_time=QTime::currentTime();
+                double hour=current_time.hour();
+                double min=current_time.minute();
+                double second=current_time.second();
+                double msec=current_time.msec();
+//                givlog->debug("decition_brain","hour: %f,min: %f,second: %f,msec: %f",
+//                                hour,min,second,msec);
             }
 
 //            brain_gps_data = gps_ins_data_;

src/decition/decition_brain_sf/decition/brain.cpp

@@ -885,6 +885,9 @@ void iv::decition::BrainDecition::run() {
             //		ODS("\n决策刹车:%f\n", decition_gps->brake);
             std::cout<<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "\n决策方向:%f\n" << decition_gps->wheel_angle << std::endl;
             std::cout<<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "\n决策速度:%f\n" << decition_gps->speed << std::endl;
+//            double decition_period=GetTickCount()-start;
+//            givlog->debug("decition_brain","period: %f",
+//                            decition_period);
             last = start;
             //decitionMaker->decideFromGPS(decition_gps, gps_data_, navigation_data);		//gps_data_为当前读到的gps位置信息  navigation_data为导航数据  decition_gps为根据前两者得出的决策速度与决策角度
             //            }

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -197,6 +197,7 @@ bool qiedianCount = false;
 static int obstacle_avoid_flag=0;
 static int front_car_id=-1;
 static int front_car_vector_id=-1;
+static bool final_brake_lock=false,brake_mode=false;
 
 //日常展示
 
@@ -1062,7 +1063,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                         double forecast_final=secSpeed*secSpeed+5;
                         int forecast_final_point=((int)forecast_final)*10+1500;
                         static int BrakePoint=-1;
-                        static bool final_brake=false,final_brake_lock=false;
+                        static bool final_brake=false;
                         static double distance_to_end=1000;
                         if(PathPoint+forecast_final_point>gpsMapLine.size())
                         {                           
@@ -1081,6 +1082,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                         {
                             final_brake=false;
                             final_brake_lock=false;
+                            brake_mode=false;
                             BrakePoint=-1;
                         }
                         if(final_brake==true){
@@ -1088,7 +1090,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                                             minDecelerate=-0.7;
                                 }else{
                                             dSpeed=min(dSpeed, 3.0);
-                                            final_brake_lock=true;                                            
+                                            final_brake_lock=true;
+                                            brake_mode=true;
                                             if(distance_to_end<0.8){
                                                             minDecelerate=-0.7;
                                             }
@@ -1295,6 +1298,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     //  dSpeed = getSpeed(gpsTraceNow);
     dSpeed =80;
+
     planTrace.clear();//Add By YuChuli, 2020.11.26
     for(int i=0;i<gpsTraceNow.size();i++){
         TracePoint pt(gpsTraceNow[i].x,gpsTraceNow[i].y);
@@ -1535,6 +1539,10 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 //            dSpeed =gpsMapLine[PathPoint]->speed*3.6;
 //        }
     }
+    //givlog->debug("decition_brain","brake_mode: %d",brake_mode);
+    if(brake_mode==true){
+        dSpeed=min(dSpeed, 3.0);
+    }
 
     if(front_car_id>0){
         static bool brake_state=false;

src/detection/detection_lidar_PointPillars_MultiHead/main.cpp

@@ -63,11 +63,13 @@ void GetLidarObj(std::vector<float> out_detections,std::vector<int> out_labels,
 {
     int i;
     int obj_size = out_detections.size()/kOutputNumBoxFeature;
-//    givlog->verbose("OBJ","object size is %d",obj_size);
+    //    givlog->verbose("OBJ","object size is %d",obj_size);
     for(i=0;i<obj_size;i++)
     {
-        if (out_scores.at(i) < 0.10) continue;
         iv::lidar::lidarobject lidarobj;
+        if (out_scores.at(i) < 0.12) continue;
+        if (out_labels.at(i) == 5) continue;
+
         lidarobj.set_tyaw(out_detections.at(i*7+6));
         iv::lidar::PointXYZ centroid;
         iv::lidar::PointXYZ * _centroid;
@@ -100,8 +102,13 @@ void GetLidarObj(std::vector<float> out_detections,std::vector<int> out_labels,
         position.set_z(out_detections.at(i*7+2));
         _position = lidarobj.mutable_position();
         _position->CopyFrom(position);
-
         lidarobj.set_mntype(out_labels.at(i));
+        // label 2  8
+        if(out_labels.at(i)==2){
+            lidarobj.set_mntype(8);
+        }else if(out_labels.at(i)==8){
+            lidarobj.set_mntype(2);
+        }
         lidarobj.set_score(out_scores.at(i));
         lidarobj.add_type_probs(out_scores.at(i));
 
@@ -111,6 +118,7 @@ void GetLidarObj(std::vector<float> out_detections,std::vector<int> out_labels,
         point_cloud.set_y(out_detections.at(i*7+1));
         point_cloud.set_z(out_detections.at(i*7+2));
         point_cloud.set_i(out_detections.at(out_labels.at(i)));
+
         _point_cloud = lidarobj.add_cloud();
         _point_cloud->CopyFrom(point_cloud);
 
@@ -122,6 +130,7 @@ void GetLidarObj(std::vector<float> out_detections,std::vector<int> out_labels,
         pld = lidarobj.mutable_dimensions();
         pld->CopyFrom(ld);
 
+        //        std::cout<<"x y z   :  "<<out_detections.at(i*7+3)<<"    "<< out_detections.at(i*7+4)<<"    "<<out_detections.at(i*7+5)<<std::endl;
         iv::lidar::lidarobject * po = lidarobjvec.add_obj();
         po->CopyFrom(lidarobj);
     }
@@ -328,8 +337,8 @@ int main(int argc, char *argv[])
 
        std::cout<<"use onnx model."<<std::endl;
     pPillars = new PointPillars(
-      0.1,
-      0.2,
+      0.15,
+      0.15,
       true,
       pfe_file,
       backbone_file,
@@ -340,8 +349,8 @@ int main(int argc, char *argv[])
     {
         std::cout<<"use engine mode."<<std::endl;
     pPillars = new PointPillars(
-      0.1,
-      0.2,
+      0.15,
+      0.15,
       false,
       pfe_trt_file,
       backbone_trt_file,

src/detection/detection_lidar_cnn_segmentation/detection_lidar_cnn_segmentation.pro

@@ -49,7 +49,7 @@ LIBS += -lpcl_io -lpcl_common
 LIBS += -lboost_system  -lavutil  -lprotobuf -lcudnn
 
 
-unix:!macx: LIBS += -L$$PWD/../../../thirdpartylib/caffe -lcaffe -lcudnn
+unix:!macx: LIBS += -L$$PWD/../../../thirdpartylib/caffe/arm64 -lcaffe -lcudnn
 
 HEADERS += \
     cluster2d.h \
@@ -70,5 +70,6 @@ INCLUDEPATH += $$PWD/caffe/proto/
 }
 
 INCLUDEPATH += $$PWD/../../include/msgtype
+LIBS += -L/usr/lib/aarch64-linux-gnu/ -lglog
 
 

src/driver/driver_rs_m1/driver_rs_m1.pro

@@ -38,5 +38,22 @@ INCLUDEPATH += /usr/include/pcl-1.7
 INCLUDEPATH += /usr/include/pcl-1.8
 INCLUDEPATH += /usr/include/eigen3
 
-INCLUDEPATH += $$PWD/../../../include/
-LIBS += -L$$PWD/../../../bin/ -lxmlparam -lmodulecomm -livlog -livfault -livexit
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+!include(../../../include/ivboost.pri ) {
+    error( "Couldn't find the ivboost.pri file!" )
+}
+
+!include(../../../include/ivyaml-cpp.pri ) {
+    error( "Couldn't find the ivyaml-cpp.pri file!" )
+}
+
+!include(../../../include/ivpcl.pri ) {
+    error( "Couldn't find the ivpcl.pri file!" )
+}

src/driver/driver_rs_m1/main.cpp

@@ -33,7 +33,7 @@ int main(int argc, char *argv[])
     QCoreApplication a(argc, argv);
     snprintf(gstr_hostip,255,"0.0.0.0");
     snprintf(gstr_port,255,"6699");//默认端口号
-    snprintf(gstr_memname,255,"lidar_pc");
+    snprintf(gstr_memname,255,"lidarpc_center");
     StartLidar_rs_m1();
 
     return a.exec();

src/driver/driver_rs_m1/rsdriver.cpp

@@ -187,8 +187,8 @@ void process_rs_M1_obs(char * strdata,int nLen)
                 for(block = 0; block < MEMS_BLOCKS_PER_PACKET; block++)
                 {
                     for (channel = 1; channel <MEMS_SCANS_PER_FIRING; channel++){
-                        point.x = NAN;
                         point.y = NAN;
+                        point.x = NAN;
                         point.z = NAN;
                         point.intensity = 0;
                         point_cloud->at(point_index, channel - 1) = point;
@@ -238,16 +238,16 @@ void process_rs_M1_obs(char * strdata,int nLen)
                             + pstr[bag*1400 + block* 52 + 33 + 8 * channel]);
                  if(Range > distance_max_thd || Range < distance_min_thd)
                  {
-                     point.x = NAN;
                      point.y = NAN;
+                     point.x = NAN;
                      point.z = NAN;
                      point.intensity = 0;
 //                     point_cloud->push_back(point);
 //                     ++point_cloud->width;
                  }else {
 
-                     point.x = i_out(2) * Range;
-                     point.y = i_out(0) * Range;
+                     point.y = i_out(2) * Range;
+                     point.x = -i_out(0) * Range;
                      point.z = i_out(1) * Range;
                      point.intensity = intensity;
 //                     point_cloud->push_back(point);

src/fusion/fusion_pointcloud_bus/fusion_pointcloud_bus.pro

@@ -22,16 +22,30 @@ INCLUDEPATH += /usr/include/pcl-1.7
 INCLUDEPATH += /usr/include/pcl-1.8
 INCLUDEPATH += /usr/include/eigen3
 
+LIBS += /usr/lib/aarch64-linux-gnu/libpcl_*.so
+
 HEADERS += \
     lidarmerge.h
 
 
 unix:LIBS += -lboost_thread -lboost_system
 
-LIBS += -lyaml-cpp
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+!include(../../../include/ivboost.pri ) {
+    error( "Couldn't find the ivboost.pri file!" )
+}
 
-LIBS += -lprotobuf
-INCLUDEPATH += $$PWD/../../include/msgtype
+!include(../../../include/ivyaml-cpp.pri ) {
+    error( "Couldn't find the ivyaml-cpp.pri file!" )
+}
 
-INCLUDEPATH += $$PWD/../../../include/
-LIBS += -L$$PWD/../../../bin/ -lxmlparam -lmodulecomm -livlog -livfault
+!include(../../../include/ivpcl.pri ) {
+    error( "Couldn't find the ivpcl.pri file!" )
+}

src/fusion/fusion_pointcloud_bus/lidarmerge.cpp

@@ -40,9 +40,10 @@ pcl::PointCloud<pcl::PointXYZI>::Ptr mergefunc(std::vector<iv::lidar_data> xvect
             {
                 pcl::PointXYZI point;
                 point = partpoint_cloud->at(j);
-                point.x = point.x + xvectorlidar[i].foff_x;
-                point.y = point.y + xvectorlidar[i].foff_y;
+                point.x = (point.x + xvectorlidar[i].foff_x)*cos(xvectorlidar[i].foff_angle) -(point.y + xvectorlidar[i].foff_y)*sin(xvectorlidar[i].foff_angle) ;
+                point.y = (point.y + xvectorlidar[i].foff_y)*cos(xvectorlidar[i].foff_angle) + (point.x + xvectorlidar[i].foff_x)*sin(xvectorlidar[i].foff_angle);
                 point.z = point.z + xvectorlidar[i].foff_z;
+
                 if(point.x>xvectorlidar[i].fmax_x)continue;
                 if(point.y>xvectorlidar[i].fmax_y)continue;
                 if(point.z>xvectorlidar[i].fmax_z)continue;
@@ -58,6 +59,7 @@ pcl::PointCloud<pcl::PointXYZI>::Ptr mergefunc(std::vector<iv::lidar_data> xvect
                 {
                     continue;
                 }
+                if(!pcl_isfinite(point.x)||!pcl_isfinite(point.y)||!pcl_isfinite(point.z)||!pcl_isfinite(point.intensity)) continue;
 
                 point_cloud->push_back(point);
                 ++point_cloud->width;

src/fusion/fusion_pointcloud_bus/lidarmerge.h

@@ -13,6 +13,7 @@ struct lidar_data
     double foff_x = 0.0;
     double foff_y = 0.0;
     double foff_z = 0.0;
+    double foff_angle = 0.0;
     bool bUpdate = false;
     double fmax_x = 0;
     double fmax_y = 0;

src/fusion/fusion_pointcloud_bus/main.cpp

@@ -3,6 +3,8 @@
 
 #include <pcl/io/io.h>
 #include <pcl/io/pcd_io.h>
+//#include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl/filters/radius_outlier_removal.h>
 
 #include <iostream>
 #include <fstream>
@@ -62,6 +64,7 @@ void dec_yaml(const char * stryamlpath)
     std::string stroffset_x;
     std::string stroffset_y;
     std::string stroffset_z;
+    std::string stroffset_angle;
     std::string strtem;
 
     double fignore_xmax = 0;
@@ -116,11 +119,13 @@ void dec_yaml(const char * stryamlpath)
                 stroffset_x = config[veclidarname[i].data()]["offset"]["x"].as<std::string>();
                 stroffset_y = config[veclidarname[i].data()]["offset"]["y"].as<std::string>();
                 stroffset_z = config[veclidarname[i].data()]["offset"]["z"].as<std::string>();
+                stroffset_angle = config[veclidarname[i].data()]["offset"]["angle"].as<std::string>();
 
                 iv::lidar_data xlidardata;
                 xlidardata.foff_x = atof(stroffset_x.data());
                 xlidardata.foff_y = atof(stroffset_y.data());
                 xlidardata.foff_z = atof(stroffset_z.data());
+                xlidardata.foff_angle = atof(stroffset_angle.data());
                 if(config[veclidarname[i].data()]["maximum"]["x"])
                 {
                     strtem = config[veclidarname[i].data()]["maximum"]["x"].as<std::string>();
@@ -256,6 +261,9 @@ void merge()
     pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
                 new pcl::PointCloud<pcl::PointXYZI>());
 
+    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(
+                new pcl::PointCloud<pcl::PointXYZI>());
+
 
 
     gMutex.lock();
@@ -266,7 +274,12 @@ void merge()
     }
     gMutex.unlock();
     point_cloud = mergefunc(xvectorlidar);
-    sharepointcloud(point_cloud,gpaout);
+    pcl::RadiusOutlierRemoval<pcl::PointXYZI> pcFilter;
+    pcFilter.setInputCloud(point_cloud);
+    pcFilter.setRadiusSearch(0.8);
+    pcFilter.setMinNeighborsInRadius(20);
+    pcFilter.filter(*cloud_filtered);
+    sharepointcloud(cloud_filtered,gpaout);
 }
 
 void mergethread()

src/fusion/lidar_radar_fusion_cnn/Tracker/Ctracker.cpp

@@ -100,7 +100,7 @@ void CTracker::UpdateTrackingState(
     {
         // Distance matrix between all tracks to all regions
         distMatrix_t costMatrix(N * M);
-        const track_t maxPossibleCost = static_cast<track_t>(currFrame.cols * currFrame.rows);
+        const track_t maxPossibleCost = 1e3;//static_cast<track_t>(currFrame.cols * currFrame.rows);
         track_t maxCost = 0;
         CreateDistaceMatrix(regions, costMatrix, maxPossibleCost, maxCost, currFrame);
 
@@ -112,6 +112,9 @@ void CTracker::UpdateTrackingState(
         {
             if (assignment[i] != -1)
             {
+#ifdef DEBUG_SHOW
+                std::cout<<costMatrix[i + assignment[i] * N]<<", ";
+#endif
                 if (costMatrix[i + assignment[i] * N] > m_settings.m_distThres)
                 {
                     assignment[i] = -1;
@@ -120,18 +123,20 @@ void CTracker::UpdateTrackingState(
             }
             else
             {
+#ifdef DEBUG_SHOW
+                std::cout<<-1<<", ";
+#endif
                 // If track have no assigned detect, then increment skipped frames counter.
                 m_tracks[i]->SkippedFrames()++;
             }
-            m_tracks[i]->m_regionID = assignment[i];
-
         }
-
+#ifdef DEBUG_SHOW
+                std::cout<<std::endl;
+#endif
         // If track didn't get detects long time, remove it.
         for (size_t i = 0; i < m_tracks.size();)
         {
             if (m_tracks[i]->SkippedFrames() > m_settings.m_maximumAllowedSkippedFrames ||
-                    m_tracks[i]->IsOutOfTheFrame() ||
                     m_tracks[i]->IsStaticTimeout(cvRound(fps * (m_settings.m_maxStaticTime - m_settings.m_minStaticTime))))
             {
                 m_tracks.erase(m_tracks.begin() + i);
@@ -156,7 +161,7 @@ void CTracker::UpdateTrackingState(
                                                         m_settings.m_useAcceleration,
                                                         m_nextTrackID++%500,
                                                         i,
-                                                        m_settings.m_filterGoal == tracking::FilterRect,
+                                                        m_settings.m_filterGoal,
                                                         m_settings.m_lostTrackType));
         }
     }
@@ -169,6 +174,7 @@ void CTracker::UpdateTrackingState(
         // If track updated less than one time, than filter state is not correct.
         if (assignment[i] != -1) // If we have assigned detect, then update using its coordinates,
         {
+            m_tracks[i]->DetectedFrames()++;
             m_tracks[i]->SkippedFrames() = 0;
             m_tracks[i]->Update(
                         regions[assignment[i]], true,
@@ -180,6 +186,7 @@ void CTracker::UpdateTrackingState(
         {
             m_tracks[i]->Update(CRegion(), false, m_settings.m_maxTraceLength, m_prevFrame, currFrame, 0);
         }
+        m_tracks[i]->m_regionID = assignment[i];
     }
 }
 
@@ -200,18 +207,18 @@ void CTracker::CreateDistaceMatrix(const regions_t& regions, distMatrix_t& costM
         const auto& track = m_tracks[i];
 
         // Calc predicted area for track
-        cv::Size_<track_t> minRadius;
-        if (m_settings.m_minAreaRadiusPix < 0)
-        {
-            minRadius.width = m_settings.m_minAreaRadiusK * track->LastRegion().m_rrect.size.width;
-            minRadius.height = m_settings.m_minAreaRadiusK * track->LastRegion().m_rrect.size.height;
-        }
-        else
-        {
-            minRadius.width = m_settings.m_minAreaRadiusPix;
-            minRadius.height = m_settings.m_minAreaRadiusPix;
-        }
-        cv::RotatedRect predictedArea = track->CalcPredictionEllipse(minRadius);
+//        cv::Size_<track_t> minRadius;
+//        if (m_settings.m_minAreaRadiusPix < 0)
+//        {
+//            minRadius.width = m_settings.m_minAreaRadiusK * track->LastRegion().m_rect.size.width;
+//            minRadius.height = m_settings.m_minAreaRadiusK * track->LastRegion().m_rect.size.height;
+//        }
+//        else
+//        {
+//            minRadius.width = m_settings.m_minAreaRadiusPix;
+//            minRadius.height = m_settings.m_minAreaRadiusPix;
+//        }
+        //cv::RotatedRect predictedArea = track->CalcPredictionEllipse(minRadius);
 
         // Calc distance between track and regions
         for (size_t j = 0; j < regions.size(); ++j)
@@ -219,13 +226,13 @@ void CTracker::CreateDistaceMatrix(const regions_t& regions, distMatrix_t& costM
             const auto& reg = regions[j];
 
             auto dist = maxPossibleCost;
-            if (m_settings.CheckType(m_tracks[i]->LastRegion().m_type, reg.m_type))
+            if (reg.m_type==-1?(m_settings.CheckType(m_tracks[i]->LastRegion().m_type_name, reg.m_type_name)):(m_settings.CheckType(m_tracks[i]->LastRegion().m_type, reg.m_type)))
             {
                 dist = 0;
                 size_t ind = 0;
                 if (m_settings.m_distType[ind] > 0.0f && ind == tracking::DistCenters)
                 {
-#if 1
+#if 0
                     track_t ellipseDist = track->IsInsideArea(reg.m_rrect.center, predictedArea);
                     if (ellipseDist > 1)
                         dist += m_settings.m_distType[ind];
@@ -239,8 +246,8 @@ void CTracker::CreateDistaceMatrix(const regions_t& regions, distMatrix_t& costM
 
                 if (m_settings.m_distType[ind] > 0.0f && ind == tracking::DistRects)
                 {
-#if 1
-                    track_t ellipseDist = track->IsInsideArea(reg.m_rrect.center, predictedArea);
+#if 0
+                    track_t ellipseDist = track->IsInsideArea(reg.m_rect.center, predictedArea);
                     if (ellipseDist < 1)
                     {
                         track_t dw = track->WidthDist(reg);
@@ -257,13 +264,25 @@ void CTracker::CreateDistaceMatrix(const regions_t& regions, distMatrix_t& costM
 #endif
                 }
                 ++ind;
-
-                if (m_settings.m_distType[ind] > 0.0f && ind == tracking::DistJaccard)
-                    dist += m_settings.m_distType[ind] * track->CalcDistJaccard(reg);
-                ++ind;
-
-                if (m_settings.m_distType[ind] > 0.0f && ind == tracking::DistHist)
-                    dist += m_settings.m_distType[ind] * track->CalcDistHist(reg, currFrame);
+                if (m_settings.m_distType[ind] > 0.0f && ind == tracking::DistRect3Ds)
+                {
+#if 0
+                    track_t ellipseDist = track->IsInsideArea(reg.m_rect.center, predictedArea);
+                    if (ellipseDist < 1)
+                    {
+                        track_t dw = track->WidthDist(reg);
+                        track_t dh = track->HeightDist(reg);
+                        dist += m_settings.m_distType[ind] * (1 - (1 - ellipseDist) * (dw + dh) * 0.5f);
+                    }
+                    else
+                    {
+                        dist += m_settings.m_distType[ind];
+                    }
+                    //std::cout << "dist = " << dist << ", ed = " << ellipseDist << ", dw = " << dw << ", dh = " << dh << std::endl;
+#else
+                    dist += m_settings.m_distType[ind] * track->CalcDistRect3D(reg);
+#endif
+                }
                 ++ind;
                 assert(ind == tracking::DistsCount);
             }

src/fusion/lidar_radar_fusion_cnn/Tracker/Ctracker.h

@@ -21,7 +21,7 @@ struct TrackerSettings
 {
     tracking::KalmanType m_kalmanType = tracking::KalmanLinear;
     tracking::FilterGoal m_filterGoal = tracking::FilterCenter;
-    tracking::LostTrackType m_lostTrackType = tracking::TrackKCF;
+    tracking::LostTrackType m_lostTrackType = tracking::TrackNone;
     tracking::MatchType m_matchType = tracking::MatchHungrian;
 
 	std::array<track_t, tracking::DistsCount> m_distType;
@@ -95,15 +95,16 @@ struct TrackerSettings
 	/// \brief m_nearTypes
 	/// Object types that can be matched while tracking
 	///
-	std::map<std::string, std::set<std::string>> m_nearTypes;
+    std::map<int, std::set<int>> m_nearTypes;
+
+    std::map<std::string, std::set<std::string>> m_nearTypeNames;
 
 	///
 	TrackerSettings()
 	{
-		m_distType[tracking::DistCenters] = 0.0f;
-		m_distType[tracking::DistRects] = 0.0f;
-		m_distType[tracking::DistJaccard] = 0.5f;
-		m_distType[tracking::DistHist] = 0.5f;
+        m_distType[tracking::DistCenters] = 0.5f;
+        m_distType[tracking::DistRects] = 0.5f;
+        m_distType[tracking::DistRect3Ds] = 0.f;
 
 		assert(CheckDistance());
 	}
@@ -112,7 +113,7 @@ struct TrackerSettings
 	bool CheckDistance() const
 	{
 		track_t sum = std::accumulate(m_distType.begin(), m_distType.end(), 0.0f);
-		track_t maxOne = std::max(1.0f, std::fabs(sum));
+        track_t maxOne = std::max(1.0f, std::fabs(sum));
 		return std::fabs(sum - 1.0f) <= std::numeric_limits<track_t>::epsilon() * maxOne;
 	}
 
@@ -139,14 +140,14 @@ struct TrackerSettings
 	}
 
 	///
-	void AddNearTypes(const std::string& type1, const std::string& type2, bool sym)
+    void AddNearTypes(const int& type1, const int& type2, bool sym)
 	{
-		auto AddOne = [&](const std::string& type1, const std::string& type2)
+        auto AddOne = [&](const int& type1, const int& type2)
 		{
 			auto it = m_nearTypes.find(type1);
 			if (it == std::end(m_nearTypes))
 			{
-				m_nearTypes[type1] = std::set<std::string>{ type2 };
+                m_nearTypes[type1] = std::set<int>{ type2 };
 			}
 			else
 			{
@@ -155,15 +156,15 @@ struct TrackerSettings
 		};
 		AddOne(type1, type2);
 		if (sym)
-		{
+        {
 			AddOne(type2, type1);
-		}
+        }
 	}
 
 	///
-	bool CheckType(const std::string& type1, const std::string& type2) const
+    bool CheckType(const int& type1, const int& type2) const
 	{
-		bool res = type1.empty() || type2.empty() || (type1 == type2);
+        bool res = type1==-1 || type2==-1 || (type1 == type2);
 		if (!res)
 		{
 			auto it = m_nearTypes.find(type1);
@@ -174,6 +175,43 @@ struct TrackerSettings
 		}
 		return res;
 	}
+
+    ///
+    void AddNearTypes(const std::string& type1, const std::string& type2, bool sym)
+    {
+        auto AddOne = [&](const std::string& type1, const std::string& type2)
+        {
+            auto it = m_nearTypeNames.find(type1);
+            if (it == std::end(m_nearTypeNames))
+            {
+                m_nearTypeNames[type1] = std::set<std::string>{ type2 };
+            }
+            else
+            {
+                it->second.insert(type2);
+            }
+        };
+        AddOne(type1, type2);
+        if (sym)
+        {
+            AddOne(type2, type1);
+        }
+    }
+
+    ///
+    bool CheckType(const std::string& type1, const std::string& type2) const
+    {
+        bool res = type1.empty() || type2.empty() || (type1 == type2);
+        if (!res)
+        {
+            auto it = m_nearTypeNames.find(type1);
+            if (it != std::end(m_nearTypeNames))
+            {
+                res = it->second.find(type2) != std::end(it->second);
+            }
+        }
+        return res;
+    }
 };
 
 ///
@@ -199,10 +237,7 @@ public:
     ///
     bool CanGrayFrameToTrack() const
     {
-        bool needColor = (m_settings.m_lostTrackType == tracking::LostTrackType::TrackGOTURN);/* ||
-            (m_settings.m_lostTrackType == tracking::LostTrackType::TrackDAT) ||
-            (m_settings.m_lostTrackType == tracking::LostTrackType::TrackSTAPLE) ||
-            (m_settings.m_lostTrackType == tracking::LostTrackType::TrackLDES);*/
+        bool needColor = (m_settings.m_lostTrackType == tracking::LostTrackType::TrackNone);
         return !needColor;
     }
 

src/fusion/lidar_radar_fusion_cnn/Tracker/HungarianAlg.cpp

@@ -71,8 +71,8 @@ void AssignmentProblemSolver::assignmentoptimal(assignments_t& assignment, track
 	for (size_t row = 0; row < nOfElements; row++)
 	{
         track_t value = distMatrixIn[row];
-        assert(value >= 0);
-        distMatrix[row] = value;
+		assert(value >= 0);
+		distMatrix[row] = value;
 	}
 
 	// Memory allocation

src/fusion/lidar_radar_fusion_cnn/Tracker/HungarianAlg.h

@@ -23,7 +23,7 @@ TKalmanFilter::TKalmanFilter(
 }
 
 //---------------------------------------------------------------------------
-void TKalmanFilter::CreateLinear(Point_t xy0, Point_t xyv0)
+void TKalmanFilter::CreateLinear(cv::Point3f xy0, cv::Point3f xyv0)
 {
     // We don't know acceleration, so, assume it to process noise.
     // But we can guess, the range of acceleration values thich can be achieved by tracked object.
@@ -68,7 +68,6 @@ void TKalmanFilter::CreateLinear(Point_t xy0, Point_t xyv0)
 
     m_initialized = true;
 }
-
 //---------------------------------------------------------------------------
 void TKalmanFilter::CreateLinear(cv::Rect_<track_t> rect0, Point_t rectv0)
 {
@@ -132,9 +131,96 @@ void TKalmanFilter::CreateLinear(cv::Rect_<track_t> rect0, Point_t rectv0)
 
     m_initialized = true;
 }
+//---------------------------------------------------------------------------
+void TKalmanFilter::CreateLinear3D(Rect3D rect0, cv::Point3f rectv0)
+{
+    // We don't know acceleration, so, assume it to process noise.
+    // But we can guess, the range of acceleration values thich can be achieved by tracked object.
+    // Process noise. (standard deviation of acceleration: m/s^2)
+    // shows, woh much target can accelerate.
+
+    // 14 state variables (x, y, z, width, height, length, yaw, d...), 7 measurements (x, y, z, width, height, length, yaw)
+    m_linearKalman.init(14, 7, 0, El_t);
+    // Transition cv::Matrix
+    m_linearKalman.transitionMatrix = (cv::Mat_<track_t>(14, 14) <<
+                                        1, 0, 0, 0, 0, 0, 0, m_deltaTime, 0,           0,           0,           0,           0,           0,
+                                        0, 1, 0, 0, 0, 0, 0, 0,           m_deltaTime, 0,           0,           0,           0,           0,
+                                        0, 0, 1, 0, 0, 0, 0, 0,           0,           m_deltaTime, 0,           0,           0,           0,
+                                        0, 0, 0, 1, 0, 0, 0, 0,           0,           0,           m_deltaTime, 0,           0,           0,
+                                        0, 0, 0, 0, 1, 0, 0, 0,           0,           0,           0,           m_deltaTime, 0,           0,
+                                        0, 0, 0, 0, 0, 1, 0, 0,           0,           0,           0,           0,           m_deltaTime, 0,
+                                        0, 0, 0, 0, 0, 0, 1, 0,           0,           0,           0,           0,           0,           m_deltaTime,
+                                        0, 0, 0, 0, 0, 0, 0, 1,           0,           0,           0,           0,           0,           0,
+                                        0, 0, 0, 0, 0, 0, 0, 0,           1,           0,           0,           0,           0,           0,
+                                        0, 0, 0, 0, 0, 0, 0, 0,           0,           1,           0,           0,           0,           0,
+                                        0, 0, 0, 0, 0, 0, 0, 0,           0,           0,           1,           0,           0,           0,
+                                        0, 0, 0, 0, 0, 0, 0, 0,           0,           0,           0,           1,           0,           0,
+                                        0, 0, 0, 0, 0, 0, 0, 0,           0,           0,           0,           0,           1,           0,
+                                        0, 0, 0, 0, 0, 0, 0, 0,           0,           0,           0,           0,           0,           1);
+
+    // init...
+    m_linearKalman.statePre.at<track_t>(0) = rect0.center.x;      // x
+    m_linearKalman.statePre.at<track_t>(1) = rect0.center.y;      // y
+    m_linearKalman.statePre.at<track_t>(2) = rect0.center.z;      // z
+    m_linearKalman.statePre.at<track_t>(3) = rect0.size.width;  // width
+    m_linearKalman.statePre.at<track_t>(4) = rect0.size.height; // height
+    m_linearKalman.statePre.at<track_t>(5) = rect0.size.length; // length
+    m_linearKalman.statePre.at<track_t>(6) = rect0.yaw; // yaw
+    m_linearKalman.statePre.at<track_t>(7) = rectv0.x;     // dx
+    m_linearKalman.statePre.at<track_t>(8) = rectv0.y;     // dy
+    m_linearKalman.statePre.at<track_t>(9) = 0;            // dz
+    m_linearKalman.statePre.at<track_t>(10) = 0;            // dw
+    m_linearKalman.statePre.at<track_t>(11) = 0;            // dh
+    m_linearKalman.statePre.at<track_t>(12) = 0;            // dl
+    m_linearKalman.statePre.at<track_t>(13) = 0;            // dyaw
+
+    m_linearKalman.statePost.at<track_t>(0) = rect0.center.x;
+    m_linearKalman.statePost.at<track_t>(1) = rect0.center.y;
+    m_linearKalman.statePost.at<track_t>(2) = rect0.center.z;
+    m_linearKalman.statePost.at<track_t>(3) = rect0.size.width;
+    m_linearKalman.statePost.at<track_t>(4) = rect0.size.height;
+    m_linearKalman.statePost.at<track_t>(5) = rect0.size.length;
+    m_linearKalman.statePost.at<track_t>(6) = rect0.yaw;
+    m_linearKalman.statePost.at<track_t>(7) = rectv0.x;
+    m_linearKalman.statePost.at<track_t>(8) = rectv0.y;
+    m_linearKalman.statePost.at<track_t>(9) = 0;
+    m_linearKalman.statePost.at<track_t>(10) = 0;
+    m_linearKalman.statePost.at<track_t>(11) = 0;
+    m_linearKalman.statePost.at<track_t>(12) = 0;
+    m_linearKalman.statePost.at<track_t>(13) = 0;
+
+    cv::setIdentity(m_linearKalman.measurementMatrix);
+
+    track_t n1 = pow(m_deltaTime, 4.f) / 4.f;
+    track_t n2 = pow(m_deltaTime, 3.f) / 2.f;
+    track_t n3 = pow(m_deltaTime, 2.f);
+    m_linearKalman.processNoiseCov = (cv::Mat_<track_t>(14, 14) <<
+                                       n1, 0,  0,  0,  0,  0,  0,  n2, 0,  0,  0,  0,  0,  0,
+                                       0,  n1, 0,  0,  0,  0,  0,  0,  n2, 0,  0,  0,  0,  0,
+                                       0,  0,  n1, 0,  0,  0,  0,  0,  0,  n2, 0,  0,  0,  0,
+                                       0,  0,  0,  n1, 0,  0,  0,  0,  0,  0,  n2, 0,  0,  0,
+                                       0,  0,  0,  0,  n1, 0,  0,  0,  0,  0,  0,  n2, 0,  0,
+                                       0,  0,  0,  0,  0,  n1, 0,  0,  0,  0,  0,  0,  n2, 0,
+                                       0,  0,  0,  0,  0,  0,  n1, 0,  0,  0,  0,  0,  0,  n2,
+                                       n2, 0,  0,  0,  0,  0,  0,  n3, 0,  0,  0,  0,  0,  0,
+                                       0,  n2, 0,  0,  0,  0,  0,  0,  n3, 0,  0,  0,  0,  0,
+                                       0,  0,  n2, 0,  0,  0,  0,  0,  0,  n3, 0,  0,  0,  0,
+                                       0,  0,  0,  n2, 0,  0,  0,  0,  0,  0,  n3, 0,  0,  0,
+                                       0,  0,  0,  0,  n2, 0,  0,  0,  0,  0,  0,  n3, 0,  0,
+                                       0,  0,  0,  0,  0,  n2, 0,  0,  0,  0,  0,  0,  n3, 0,
+                                       0,  0,  0,  0,  0,  0,  n2, 0,  0,  0,  0,  0,  0,  n3);
+
+    m_linearKalman.processNoiseCov *= m_accelNoiseMag;
+
+    cv::setIdentity(m_linearKalman.measurementNoiseCov, cv::Scalar::all(0.1));
+
+    cv::setIdentity(m_linearKalman.errorCovPost, cv::Scalar::all(.1));
+
+    m_initialized = true;
+}
 
 //---------------------------------------------------------------------------
-void TKalmanFilter::CreateLinearAcceleration(Point_t xy0, Point_t xyv0)
+void TKalmanFilter::CreateLinearAcceleration(cv::Point3f xy0, cv::Point3f xyv0)
 {
 	// 6 state variables, 2 measurements
 	m_linearKalman.init(6, 2, 0, El_t);
@@ -186,9 +272,83 @@ void TKalmanFilter::CreateLinearAcceleration(Point_t xy0, Point_t xyv0)
 
 	m_initialized = true;
 }
-
 //---------------------------------------------------------------------------
 void TKalmanFilter::CreateLinearAcceleration(cv::Rect_<track_t> rect0, Point_t rectv0)
+{
+    // 12 state variables (x, y, vx, vy, ax, ay, width, height, vw, vh, aw, ah), 4 measurements (x, y, width, height)
+    m_linearKalman.init(12, 4, 0, El_t);
+    // Transition cv::Matrix
+    const track_t dt = m_deltaTime;
+    const track_t dt2 = 0.5f * m_deltaTime * m_deltaTime;
+    m_linearKalman.transitionMatrix = (cv::Mat_<track_t>(12, 12) <<
+        1, 0, 0, 0, dt, 0,  0,  0,  dt2, 0,   dt2, 0,
+        0, 1, 0, 0, 0,  dt, 0,  0,  0,   dt2, 0,   dt2,
+        0, 0, 1, 0, 0,  0,  dt, 0,  0,   0,   dt2, 0,
+        0, 0, 0, 1, 0,  0,  0,  dt, 0,   0,   0,   dt2,
+        0, 0, 0, 0, 1,  0,  0,  0,  dt,  0,   0,   0,
+        0, 0, 0, 0, 0,  1,  0,  0,  0,   dt,  0,   0,
+        0, 0, 0, 0, 0,  0,  1,  0,  0,   0,   dt,  0,
+        0, 0, 0, 0, 0,  0,  0,  1,  0,   0,   0,   dt,
+        0, 0, 0, 0, 0,  0,  0,  0,  1,   0,   0,   0,
+        0, 0, 0, 0, 0,  0,  0,  0,  0,   1,   0,   0,
+        0, 0, 0, 0, 0,  0,  0,  0,  0,   0,   0,   1);
+
+    // init...
+    m_linearKalman.statePre.at<track_t>(0) = rect0.x;      // x
+    m_linearKalman.statePre.at<track_t>(1) = rect0.y;      // y
+    m_linearKalman.statePre.at<track_t>(2) = rect0.width;  // width
+    m_linearKalman.statePre.at<track_t>(3) = rect0.height; // height
+    m_linearKalman.statePre.at<track_t>(4) = rectv0.x;     // dx
+    m_linearKalman.statePre.at<track_t>(5) = rectv0.y;     // dy
+    m_linearKalman.statePre.at<track_t>(6) = 0;            // dw
+    m_linearKalman.statePre.at<track_t>(7) = 0;            // dh
+    m_linearKalman.statePre.at<track_t>(8) = 0;            // ax
+    m_linearKalman.statePre.at<track_t>(9) = 0;            // ay
+    m_linearKalman.statePre.at<track_t>(10) = 0;           // aw
+    m_linearKalman.statePre.at<track_t>(11) = 0;           // ah
+
+    m_linearKalman.statePost.at<track_t>(0) = rect0.x;
+    m_linearKalman.statePost.at<track_t>(1) = rect0.y;
+    m_linearKalman.statePost.at<track_t>(2) = rect0.width;
+    m_linearKalman.statePost.at<track_t>(3) = rect0.height;
+    m_linearKalman.statePost.at<track_t>(4) = rectv0.x;
+    m_linearKalman.statePost.at<track_t>(5) = rectv0.y;
+    m_linearKalman.statePost.at<track_t>(6) = 0;
+    m_linearKalman.statePost.at<track_t>(7) = 0;
+    m_linearKalman.statePost.at<track_t>(8) = 0;
+    m_linearKalman.statePost.at<track_t>(9) = 0;
+    m_linearKalman.statePost.at<track_t>(10) = 0;
+    m_linearKalman.statePost.at<track_t>(11) = 0;
+
+    cv::setIdentity(m_linearKalman.measurementMatrix);
+
+    track_t n1 = pow(m_deltaTime, 4.f) / 4.f;
+    track_t n2 = pow(m_deltaTime, 3.f) / 2.f;
+    track_t n3 = pow(m_deltaTime, 2.f);
+    m_linearKalman.processNoiseCov = (cv::Mat_<track_t>(12, 12) <<
+        n1, 0, 0, 0, n2, 0, 0, 0, n2, 0, n2,
+        0, n1, 0, 0, 0, n2, 0, 0, 0, n2, 0, n2,
+        0, 0, n1, 0, 0, 0, n2, 0, 0, 0, n2, 0,
+        0, 0, 0, n1, 0, 0, 0, n2, 0, 0, 0, n2,
+        n2, 0, 0, 0, n3, 0, 0, 0, n3, 0, n3, 0,
+        0, n2, 0, 0, 0, n3, 0, 0, 0, n3, 0, n3,
+        0, 0, n2, 0, 0, 0, n3, 0, 0, 0, n3, 0,
+        0, 0, 0, n2, 0, 0, 0, n3, 0, 0, 0, n3,
+        n2, 0, 0, 0, n3, 0, 0, 0, n3, 0, 0, 0,
+        0, n2, 0, 0, 0, n3, 0, 0, 0, n3, 0, 0,
+        0, 0, n2, 0, 0, 0, n3, 0, 0, 0, n3, 0,
+        0, 0, 0, n2, 0, 0, 0, n3, 0, 0, 0, n3);
+
+    m_linearKalman.processNoiseCov *= m_accelNoiseMag;
+
+    cv::setIdentity(m_linearKalman.measurementNoiseCov, cv::Scalar::all(0.1));
+
+    cv::setIdentity(m_linearKalman.errorCovPost, cv::Scalar::all(.1));
+
+    m_initialized = true;
+}
+//---------------------------------------------------------------------------
+void TKalmanFilter::CreateLinearAcceleration3D(Rect3D rect0, cv::Point3f rectv0)
 {
 	// 12 state variables (x, y, vx, vy, ax, ay, width, height, vw, vh, aw, ah), 4 measurements (x, y, width, height)
 	m_linearKalman.init(12, 4, 0, El_t);
@@ -209,10 +369,10 @@ void TKalmanFilter::CreateLinearAcceleration(cv::Rect_<track_t> rect0, Point_t r
 		0, 0, 0, 0, 0,  0,  0,  0,  0,   0,   0,   1);
 
 	// init...
-	m_linearKalman.statePre.at<track_t>(0) = rect0.x;      // x
-	m_linearKalman.statePre.at<track_t>(1) = rect0.y;      // y
-	m_linearKalman.statePre.at<track_t>(2) = rect0.width;  // width
-	m_linearKalman.statePre.at<track_t>(3) = rect0.height; // height
+    m_linearKalman.statePre.at<track_t>(0) = rect0.center.x;      // x
+    m_linearKalman.statePre.at<track_t>(1) = rect0.center.y;      // y
+    m_linearKalman.statePre.at<track_t>(2) = rect0.size.width;  // width
+    m_linearKalman.statePre.at<track_t>(3) = rect0.size.height; // height
 	m_linearKalman.statePre.at<track_t>(4) = rectv0.x;     // dx
 	m_linearKalman.statePre.at<track_t>(5) = rectv0.y;     // dy
 	m_linearKalman.statePre.at<track_t>(6) = 0;            // dw
@@ -222,10 +382,10 @@ void TKalmanFilter::CreateLinearAcceleration(cv::Rect_<track_t> rect0, Point_t r
 	m_linearKalman.statePre.at<track_t>(10) = 0;           // aw
 	m_linearKalman.statePre.at<track_t>(11) = 0;           // ah
 
-	m_linearKalman.statePost.at<track_t>(0) = rect0.x;
-	m_linearKalman.statePost.at<track_t>(1) = rect0.y;
-	m_linearKalman.statePost.at<track_t>(2) = rect0.width;
-	m_linearKalman.statePost.at<track_t>(3) = rect0.height;
+    m_linearKalman.statePost.at<track_t>(0) = rect0.center.x;
+    m_linearKalman.statePost.at<track_t>(1) = rect0.center.y;
+    m_linearKalman.statePost.at<track_t>(2) = rect0.size.width;
+    m_linearKalman.statePost.at<track_t>(3) = rect0.size.height;
 	m_linearKalman.statePost.at<track_t>(4) = rectv0.x;
 	m_linearKalman.statePost.at<track_t>(5) = rectv0.y;
 	m_linearKalman.statePost.at<track_t>(6) = 0;
@@ -263,275 +423,8 @@ void TKalmanFilter::CreateLinearAcceleration(cv::Rect_<track_t> rect0, Point_t r
 	m_initialized = true;
 }
 
-#ifdef USE_OCV_UKF
 //---------------------------------------------------------------------------
-class AcceleratedModel: public cv::tracking::UkfSystemModel
-{
-public:
-    AcceleratedModel(track_t deltaTime, bool rectModel)
-        :
-          cv::tracking::UkfSystemModel(),
-          m_deltaTime(deltaTime),
-          m_rectModel(rectModel)
-    {
-
-    }
-
-    void stateConversionFunction(const cv::Mat& x_k, const cv::Mat& u_k, const cv::Mat& v_k, cv::Mat& x_kplus1)
-    {
-        track_t x0 = x_k.at<track_t>(0, 0);
-        track_t y0 = x_k.at<track_t>(1, 0);
-        track_t vx0 = x_k.at<track_t>(2, 0);
-        track_t vy0 = x_k.at<track_t>(3, 0);
-        track_t ax0 = x_k.at<track_t>(4, 0);
-        track_t ay0 = x_k.at<track_t>(5, 0);
-
-        x_kplus1.at<track_t>(0, 0) = x0 + vx0 * m_deltaTime + ax0 * sqr(m_deltaTime) / 2;
-        x_kplus1.at<track_t>(1, 0) = y0 + vy0 * m_deltaTime + ay0 * sqr(m_deltaTime) / 2;
-        x_kplus1.at<track_t>(2, 0) = vx0 + ax0 * m_deltaTime;
-        x_kplus1.at<track_t>(3, 0) = vy0 + ay0 * m_deltaTime;
-        x_kplus1.at<track_t>(4, 0) = ax0;
-        x_kplus1.at<track_t>(5, 0) = ay0;
-
-        if (m_rectModel)
-        {
-            x_kplus1.at<track_t>(6, 0) = x_k.at<track_t>(6, 0);
-            x_kplus1.at<track_t>(7, 0) = x_k.at<track_t>(7, 0);
-        }
-
-        if (v_k.size() == u_k.size())
-        {
-            x_kplus1 += v_k + u_k;
-        }
-        else
-        {
-            x_kplus1 += v_k;
-        }
-    }
-
-    void measurementFunction(const cv::Mat& x_k, const cv::Mat& n_k, cv::Mat& z_k)
-    {
-        track_t x0 = x_k.at<track_t>(0, 0);
-        track_t y0 = x_k.at<track_t>(1, 0);
-        track_t vx0 = x_k.at<track_t>(2, 0);
-        track_t vy0 = x_k.at<track_t>(3, 0);
-        track_t ax0 = x_k.at<track_t>(4, 0);
-        track_t ay0 = x_k.at<track_t>(5, 0);
-
-        z_k.at<track_t>(0, 0) = x0 + vx0 * m_deltaTime + ax0 * sqr(m_deltaTime) / 2 + n_k.at<track_t>(0, 0);
-        z_k.at<track_t>(1, 0) = y0 + vy0 * m_deltaTime + ay0 * sqr(m_deltaTime) / 2 + n_k.at<track_t>(1, 0);
-
-        if (m_rectModel)
-        {
-            z_k.at<track_t>(2, 0) = x_k.at<track_t>(6, 0);
-            z_k.at<track_t>(3, 0) = x_k.at<track_t>(7, 0);
-        }
-    }
-
-private:
-    track_t m_deltaTime;
-    bool m_rectModel;
-};
-
-//---------------------------------------------------------------------------
-void TKalmanFilter::CreateUnscented(Point_t xy0, Point_t xyv0)
-{
-    int MP = 2;
-    int DP = 6;
-    int CP = 0;
-
-    cv::Mat processNoiseCov = cv::Mat::zeros(DP, DP, Mat_t(1));
-    processNoiseCov.at<track_t>(0, 0) = 1e-14f;
-    processNoiseCov.at<track_t>(1, 1) = 1e-14f;
-    processNoiseCov.at<track_t>(2, 2) = 1e-6f;
-    processNoiseCov.at<track_t>(3, 3) = 1e-6f;
-    processNoiseCov.at<track_t>(4, 4) = 1e-6f;
-    processNoiseCov.at<track_t>(5, 5) = 1e-6f;
-
-    cv::Mat measurementNoiseCov = cv::Mat::zeros(MP, MP, Mat_t(1));
-    measurementNoiseCov.at<track_t>(0, 0) = 1e-6f;
-    measurementNoiseCov.at<track_t>(1, 1) = 1e-6f;
-
-    cv::Mat initState(DP, 1, Mat_t(1));
-    initState.at<track_t>(0, 0) = xy0.x;
-    initState.at<track_t>(1, 0) = xy0.y;
-    initState.at<track_t>(2, 0) = xyv0.x;
-    initState.at<track_t>(3, 0) = xyv0.y;
-    initState.at<track_t>(4, 0) = 0;
-    initState.at<track_t>(5, 0) = 0;
-
-    cv::Mat P = 1e-6f * cv::Mat::eye(DP, DP, Mat_t(1));
-
-    cv::Ptr<AcceleratedModel> model(new AcceleratedModel(m_deltaTime, false));
-    cv::tracking::UnscentedKalmanFilterParams params(DP, MP, CP, 0, 0, model);
-    params.dataType = Mat_t(1);
-    params.stateInit = initState.clone();
-    params.errorCovInit = P.clone();
-    params.measurementNoiseCov = measurementNoiseCov.clone();
-    params.processNoiseCov = processNoiseCov.clone();
-
-    params.alpha = 1.0;
-    params.beta = 2.0;
-    params.k = -2.0;
-
-    m_uncsentedKalman = cv::tracking::createUnscentedKalmanFilter(params);
-
-    m_initialized = true;
-}
-
-//---------------------------------------------------------------------------
-void TKalmanFilter::CreateUnscented(cv::Rect_<track_t> rect0, Point_t rectv0)
-{
-    int MP = 4;
-    int DP = 8;
-    int CP = 0;
-
-    cv::Mat processNoiseCov = cv::Mat::zeros(DP, DP, Mat_t(1));
-    processNoiseCov.at<track_t>(0, 0) = 1e-3f;
-    processNoiseCov.at<track_t>(1, 1) = 1e-3f;
-    processNoiseCov.at<track_t>(2, 2) = 1e-3f;
-    processNoiseCov.at<track_t>(3, 3) = 1e-3f;
-    processNoiseCov.at<track_t>(4, 4) = 1e-3f;
-    processNoiseCov.at<track_t>(5, 5) = 1e-3f;
-    processNoiseCov.at<track_t>(6, 6) = 1e-3f;
-    processNoiseCov.at<track_t>(7, 7) = 1e-3f;
-
-    cv::Mat measurementNoiseCov = cv::Mat::zeros(MP, MP, Mat_t(1));
-    measurementNoiseCov.at<track_t>(0, 0) = 1e-3f;
-    measurementNoiseCov.at<track_t>(1, 1) = 1e-3f;
-    measurementNoiseCov.at<track_t>(2, 2) = 1e-3f;
-    measurementNoiseCov.at<track_t>(3, 3) = 1e-3f;
-
-    cv::Mat initState(DP, 1, Mat_t(1));
-    initState.at<track_t>(0, 0) = rect0.x;
-    initState.at<track_t>(1, 0) = rect0.y;
-    initState.at<track_t>(2, 0) = rectv0.x;
-    initState.at<track_t>(3, 0) = rectv0.y;
-    initState.at<track_t>(4, 0) = 0;
-    initState.at<track_t>(5, 0) = 0;
-    initState.at<track_t>(6, 0) = rect0.width;
-    initState.at<track_t>(7, 0) = rect0.height;
-
-    cv::Mat P = 1e-3f * cv::Mat::eye(DP, DP, Mat_t(1));
-
-    cv::Ptr<AcceleratedModel> model(new AcceleratedModel(m_deltaTime, true));
-    cv::tracking::UnscentedKalmanFilterParams params(DP, MP, CP, 0, 0, model);
-    params.dataType = Mat_t(1);
-    params.stateInit = initState.clone();
-    params.errorCovInit = P.clone();
-    params.measurementNoiseCov = measurementNoiseCov.clone();
-    params.processNoiseCov = processNoiseCov.clone();
-
-    params.alpha = 1;
-    params.beta = 2.0;
-    params.k = -2.0;
-
-    m_uncsentedKalman = cv::tracking::createUnscentedKalmanFilter(params);
-
-    m_initialized = true;
-}
-
-//---------------------------------------------------------------------------
-void TKalmanFilter::CreateAugmentedUnscented(Point_t xy0, Point_t xyv0)
-{
-    int MP = 2;
-    int DP = 6;
-    int CP = 0;
-
-    cv::Mat processNoiseCov = cv::Mat::zeros(DP, DP, Mat_t(1));
-    processNoiseCov.at<track_t>(0, 0) = 1e-14f;
-    processNoiseCov.at<track_t>(1, 1) = 1e-14f;
-    processNoiseCov.at<track_t>(2, 2) = 1e-6f;
-    processNoiseCov.at<track_t>(3, 3) = 1e-6f;
-    processNoiseCov.at<track_t>(4, 4) = 1e-6f;
-    processNoiseCov.at<track_t>(5, 5) = 1e-6f;
-
-    cv::Mat measurementNoiseCov = cv::Mat::zeros(MP, MP, Mat_t(1));
-    measurementNoiseCov.at<track_t>(0, 0) = 1e-6f;
-    measurementNoiseCov.at<track_t>(1, 1) = 1e-6f;
-
-    cv::Mat initState(DP, 1, Mat_t(1));
-    initState.at<track_t>(0, 0) = xy0.x;
-    initState.at<track_t>(1, 0) = xy0.y;
-    initState.at<track_t>(2, 0) = xyv0.x;
-    initState.at<track_t>(3, 0) = xyv0.y;
-    initState.at<track_t>(4, 0) = 0;
-    initState.at<track_t>(5, 0) = 0;
-
-    cv::Mat P = 1e-6f * cv::Mat::eye(DP, DP, Mat_t(1));
-
-    cv::Ptr<AcceleratedModel> model(new AcceleratedModel(m_deltaTime, false));
-    cv::tracking::AugmentedUnscentedKalmanFilterParams params(DP, MP, CP, 0, 0, model);
-    params.dataType = Mat_t(1);
-    params.stateInit = initState.clone();
-    params.errorCovInit = P.clone();
-    params.measurementNoiseCov = measurementNoiseCov.clone();
-    params.processNoiseCov = processNoiseCov.clone();
-
-    params.alpha = 1;
-    params.beta = 2.0;
-    params.k = -2.0;
-
-    m_uncsentedKalman = cv::tracking::createAugmentedUnscentedKalmanFilter(params);
-
-    m_initialized = true;
-}
-
-//---------------------------------------------------------------------------
-void TKalmanFilter::CreateAugmentedUnscented(cv::Rect_<track_t> rect0, Point_t rectv0)
-{
-    int MP = 4;
-    int DP = 8;
-    int CP = 0;
-
-    cv::Mat processNoiseCov = cv::Mat::zeros(DP, DP, Mat_t(1));
-    processNoiseCov.at<track_t>(0, 0) = 1e-3f;
-    processNoiseCov.at<track_t>(1, 1) = 1e-3f;
-    processNoiseCov.at<track_t>(2, 2) = 1e-3f;
-    processNoiseCov.at<track_t>(3, 3) = 1e-3f;
-    processNoiseCov.at<track_t>(4, 4) = 1e-3f;
-    processNoiseCov.at<track_t>(5, 5) = 1e-3f;
-    processNoiseCov.at<track_t>(6, 6) = 1e-3f;
-    processNoiseCov.at<track_t>(7, 7) = 1e-3f;
-
-    cv::Mat measurementNoiseCov = cv::Mat::zeros(MP, MP, Mat_t(1));
-    measurementNoiseCov.at<track_t>(0, 0) = 1e-3f;
-    measurementNoiseCov.at<track_t>(1, 1) = 1e-3f;
-    measurementNoiseCov.at<track_t>(2, 2) = 1e-3f;
-    measurementNoiseCov.at<track_t>(3, 3) = 1e-3f;
-
-    cv::Mat initState(DP, 1, Mat_t(1));
-    initState.at<track_t>(0, 0) = rect0.x;
-    initState.at<track_t>(1, 0) = rect0.y;
-    initState.at<track_t>(2, 0) = rectv0.x;
-    initState.at<track_t>(3, 0) = rectv0.y;
-    initState.at<track_t>(4, 0) = 0;
-    initState.at<track_t>(5, 0) = 0;
-    initState.at<track_t>(6, 0) = rect0.width;
-    initState.at<track_t>(7, 0) = rect0.height;
-
-    cv::Mat P = 1e-3f * cv::Mat::eye(DP, DP, Mat_t(1));
-
-    cv::Ptr<AcceleratedModel> model(new AcceleratedModel(m_deltaTime, true));
-    cv::tracking::AugmentedUnscentedKalmanFilterParams params(DP, MP, CP, 0, 0, model);
-    params.dataType = Mat_t(1);
-    params.stateInit = initState.clone();
-    params.errorCovInit = P.clone();
-    params.measurementNoiseCov = measurementNoiseCov.clone();
-    params.processNoiseCov = processNoiseCov.clone();
-
-    params.alpha = 1;
-    params.beta = 2.0;
-    params.k = -2.0;
-
-    m_uncsentedKalman = cv::tracking::createAugmentedUnscentedKalmanFilter(params);
-
-    m_initialized = true;
-}
-#endif
-
-//---------------------------------------------------------------------------
-Point_t TKalmanFilter::GetPointPrediction()
+cv::Point3f TKalmanFilter::GetPointPrediction()
 {
     if (m_initialized)
     {
@@ -542,19 +435,9 @@ Point_t TKalmanFilter::GetPointPrediction()
         case tracking::KalmanLinear:
             prediction = m_linearKalman.predict();
             break;
-
-        case tracking::KalmanUnscented:
-        case tracking::KalmanAugmentedUnscented:
-#ifdef USE_OCV_UKF
-            prediction = m_uncsentedKalman->predict();
-#else
-            prediction = m_linearKalman.predict();
-            std::cerr << "UnscentedKalmanFilter was disabled in CMAKE! Set KalmanLinear in constructor." << std::endl;
-#endif
-            break;
         }
 
-        m_lastPointResult = Point_t(prediction.at<track_t>(0), prediction.at<track_t>(1));
+        m_lastPointResult = cv::Point3f(prediction.at<track_t>(0), prediction.at<track_t>(1), prediction.at<track_t>(2));
     }
     else
     {
@@ -564,7 +447,7 @@ Point_t TKalmanFilter::GetPointPrediction()
 }
 
 //---------------------------------------------------------------------------
-Point_t TKalmanFilter::Update(Point_t pt, bool dataCorrect)
+cv::Point3f TKalmanFilter::Update(cv::Point3f pt, bool dataCorrect)
 {
     if (!m_initialized)
     {
@@ -583,8 +466,8 @@ Point_t TKalmanFilter::Update(Point_t pt, bool dataCorrect)
             track_t ky = 0;
             track_t by = 0;
             get_lin_regress_params(m_initialPoints, 0, MIN_INIT_VALS, kx, bx, ky, by);//predict p,v
-            Point_t xy0(kx * (MIN_INIT_VALS - 1) + bx, ky * (MIN_INIT_VALS - 1) + by);
-            Point_t xyv0(kx, ky);
+            cv::Point3f xy0(kx * (MIN_INIT_VALS - 1) + bx, ky * (MIN_INIT_VALS - 1) + by, m_lastPointResult.z);
+            cv::Point3f xyv0(kx, ky,0);
 
             switch (m_type)
             {
@@ -594,30 +477,6 @@ Point_t TKalmanFilter::Update(Point_t pt, bool dataCorrect)
 				else
 					CreateLinear(xy0, xyv0);
                 break;
-
-            case tracking::KalmanUnscented:
-#ifdef USE_OCV_UKF
-                CreateUnscented(xy0, xyv0);
-#else
-				if (m_useAcceleration)
-					CreateLinearAcceleration(xy0, xyv0);
-				else
-					CreateLinear(xy0, xyv0);
-                std::cerr << "UnscentedKalmanFilter was disabled in CMAKE! Set KalmanLinear in constructor." << std::endl;
-#endif
-                break;
-
-            case tracking::KalmanAugmentedUnscented:
-#ifdef USE_OCV_UKF
-                CreateAugmentedUnscented(xy0, xyv0);
-#else
-				if (m_useAcceleration)
-					CreateLinearAcceleration(xy0, xyv0);
-				else
-					CreateLinear(xy0, xyv0);
-                std::cerr << "AugmentedUnscentedKalmanFilter was disabled in CMAKE! Set KalmanLinear in constructor." << std::endl;
-#endif
-                break;
             }
             m_lastDist = 0;
         }
@@ -663,16 +522,6 @@ Point_t TKalmanFilter::Update(Point_t pt, bool dataCorrect)
 			}
             break;
         }
-
-        case tracking::KalmanUnscented:
-        case tracking::KalmanAugmentedUnscented:
-#ifdef USE_OCV_UKF
-            estimated = m_uncsentedKalman->correct(measurement);
-#else
-            estimated = m_linearKalman.correct(measurement);
-            std::cerr << "UnscentedKalmanFilter was disabled in CMAKE! Set KalmanLinear in constructor." << std::endl;
-#endif
-            break;
         }
 
         m_lastPointResult.x = estimated.at<track_t>(0);   //update using measurements
@@ -687,7 +536,6 @@ Point_t TKalmanFilter::Update(Point_t pt, bool dataCorrect)
     }
     return m_lastPointResult;
 }
-
 //---------------------------------------------------------------------------
 cv::Rect TKalmanFilter::GetRectPrediction()
 {
@@ -700,16 +548,6 @@ cv::Rect TKalmanFilter::GetRectPrediction()
         case tracking::KalmanLinear:
             prediction = m_linearKalman.predict();
             break;
-
-        case tracking::KalmanUnscented:
-        case tracking::KalmanAugmentedUnscented:
-#ifdef USE_OCV_UKF
-            prediction = m_uncsentedKalman->predict();
-#else
-            prediction = m_linearKalman.predict();
-            std::cerr << "UnscentedKalmanFilter was disabled in CMAKE! Set KalmanLinear in constructor." << std::endl;
-#endif
-            break;
         }
 
         m_lastRectResult = cv::Rect_<track_t>(prediction.at<track_t>(0), prediction.at<track_t>(1), prediction.at<track_t>(2), prediction.at<track_t>(3));
@@ -761,34 +599,10 @@ cv::Rect TKalmanFilter::Update(cv::Rect rect, bool dataCorrect)
             switch (m_type)
             {
             case tracking::KalmanLinear:
-				if (m_useAcceleration)
-					CreateLinearAcceleration(rect0, rectv0);
-				else
-					CreateLinear(rect0, rectv0);
-                break;
-
-            case tracking::KalmanUnscented:
-#ifdef USE_OCV_UKF
-                CreateUnscented(rect0, rectv0);
-#else
-				if (m_useAcceleration)
-					CreateLinearAcceleration(rect0, rectv0);
-				else
-					CreateLinear(rect0, rectv0);
-                std::cerr << "UnscentedKalmanFilter was disabled in CMAKE! Set KalmanLinear in constructor." << std::endl;
-#endif
-                break;
-
-            case tracking::KalmanAugmentedUnscented:
-#ifdef USE_OCV_UKF
-                CreateAugmentedUnscented(rect0, rectv0);
-#else
-				if (m_useAcceleration)
-					CreateLinearAcceleration(rect0, rectv0);
-				else
-					CreateLinear(rect0, rectv0);
-                std::cerr << "AugmentedUnscentedKalmanFilter was disabled in CMAKE! Set KalmanLinear in constructor." << std::endl;
-#endif
+                if (m_useAcceleration)
+                    CreateLinearAcceleration(rect0, rectv0);
+                else
+                    CreateLinear(rect0, rectv0);
                 break;
             }
         }
@@ -824,10 +638,167 @@ cv::Rect TKalmanFilter::Update(cv::Rect rect, bool dataCorrect)
             m_lastRectResult.width = estimated.at<track_t>(2);
             m_lastRectResult.height = estimated.at<track_t>(3);
 
+            // Inertia correction
+            if (!m_useAcceleration)
+            {
+                track_t currDist = sqrtf(sqr(estimated.at<track_t>(0) - rect.x) + sqr(estimated.at<track_t>(1) - rect.y) + sqr(estimated.at<track_t>(2) - rect.width) + sqr(estimated.at<track_t>(3) - rect.height));
+                if (currDist > m_lastDist)
+                {
+                    m_deltaTime = std::min(m_deltaTime + m_deltaStep, m_deltaTimeMax);
+                }
+                else
+                {
+                    m_deltaTime = std::max(m_deltaTime - m_deltaStep, m_deltaTimeMin);
+                }
+                m_lastDist = currDist;
+
+                m_linearKalman.transitionMatrix.at<track_t>(0, 4) = m_deltaTime;
+                m_linearKalman.transitionMatrix.at<track_t>(1, 5) = m_deltaTime;
+                m_linearKalman.transitionMatrix.at<track_t>(2, 6) = m_deltaTime;
+                m_linearKalman.transitionMatrix.at<track_t>(3, 7) = m_deltaTime;
+            }
+            break;
+        }
+        }
+    }
+    else
+    {
+        if (dataCorrect)
+        {
+            m_lastRectResult.x = static_cast<track_t>(rect.x);
+            m_lastRectResult.y = static_cast<track_t>(rect.y);
+            m_lastRectResult.width = static_cast<track_t>(rect.width);
+            m_lastRectResult.height = static_cast<track_t>(rect.height);
+        }
+    }
+    return cv::Rect(static_cast<int>(m_lastRectResult.x), static_cast<int>(m_lastRectResult.y), static_cast<int>(m_lastRectResult.width), static_cast<int>(m_lastRectResult.height));
+}
+//---------------------------------------------------------------------------
+Rect3D TKalmanFilter::GetRect3DPrediction()
+{
+    if (m_initialized)
+    {
+        cv::Mat prediction;
+
+        switch (m_type)
+        {
+        case tracking::KalmanLinear:
+            prediction = m_linearKalman.predict();
+            break;
+        }
+
+        m_lastRect3DResult = Rect3D(cv::Point3f(prediction.at<track_t>(0), prediction.at<track_t>(1), prediction.at<track_t>(2)), Size3D(prediction.at<track_t>(3), prediction.at<track_t>(4), prediction.at<track_t>(5)), prediction.at<track_t>(6));
+    }
+    else
+    {
+
+    }
+    return m_lastRect3DResult;
+}
+
+//---------------------------------------------------------------------------
+Rect3D TKalmanFilter::Update(Rect3D rect, bool dataCorrect)
+{
+    if (!m_initialized)
+    {
+        if (m_initialRect3Ds.size() < MIN_INIT_VALS)
+        {
+            if (dataCorrect)
+            {
+                m_initialRect3Ds.push_back(rect);
+                m_lastRect3DResult.center.x = static_cast<track_t>(rect.center.x);
+                m_lastRect3DResult.center.y = static_cast<track_t>(rect.center.y);
+                m_lastRect3DResult.center.z = static_cast<track_t>(rect.center.z);
+                m_lastRect3DResult.size.width = static_cast<track_t>(rect.size.width);
+                m_lastRect3DResult.size.height = static_cast<track_t>(rect.size.height);
+                m_lastRect3DResult.size.length = static_cast<track_t>(rect.size.length);
+                m_lastRect3DResult.yaw = static_cast<track_t>(rect.yaw);
+            }
+        }
+        if (m_initialRect3Ds.size() == MIN_INIT_VALS)
+        {
+            std::vector<Point_t> initialPoints;
+            cv::Point3f averageSize(0, 0, 0);
+            float averageZ = 0;
+            float averageYaw = 0;
+            for (const auto& r : m_initialRect3Ds)
+            {
+                initialPoints.emplace_back(static_cast<track_t>(r.center.x), static_cast<track_t>(r.center.y));
+                averageZ += r.center.z;
+                averageSize.x += r.size.width;
+                averageSize.y += r.size.height;
+                averageSize.z += r.size.length;
+                averageYaw += r.yaw;
+            }
+            averageZ /= MIN_INIT_VALS;
+            averageSize.x /= MIN_INIT_VALS;
+            averageSize.y /= MIN_INIT_VALS;
+            averageSize.z /= MIN_INIT_VALS;
+            averageYaw /= MIN_INIT_VALS;
+
+            track_t kx = 0;
+            track_t bx = 0;
+            track_t ky = 0;
+            track_t by = 0;
+            get_lin_regress_params(initialPoints, 0, MIN_INIT_VALS, kx, bx, ky, by);
+            Rect3D rect0(cv::Point3f(kx * (MIN_INIT_VALS - 1) + bx, ky * (MIN_INIT_VALS - 1) + by, averageZ), Size3D(averageSize.x, averageSize.y,averageSize.z), averageYaw);
+            cv::Point3f rectv0(kx, ky, 0);
+
+            switch (m_type)
+            {
+            case tracking::KalmanLinear:
+				if (m_useAcceleration)
+                    CreateLinearAcceleration3D(rect0, rectv0);
+				else
+                    CreateLinear3D(rect0, rectv0);
+                break;
+            }
+        }
+    }
+
+    if (m_initialized)
+    {
+        cv::Mat measurement(7, 1, Mat_t(1));
+        if (!dataCorrect)
+        {
+            measurement.at<track_t>(0) = m_lastRect3DResult.center.x;  // update using prediction
+            measurement.at<track_t>(1) = m_lastRect3DResult.center.y;
+            measurement.at<track_t>(2) = m_lastRect3DResult.center.z;
+            measurement.at<track_t>(3) = m_lastRect3DResult.size.width;
+            measurement.at<track_t>(4) = m_lastRect3DResult.size.height;
+            measurement.at<track_t>(5) = m_lastRect3DResult.size.length;
+            measurement.at<track_t>(6) = m_lastRect3DResult.yaw;
+        }
+        else
+        {
+            measurement.at<track_t>(0) = static_cast<track_t>(rect.center.x);  // update using measurements
+            measurement.at<track_t>(1) = static_cast<track_t>(rect.center.y);
+            measurement.at<track_t>(2) = static_cast<track_t>(rect.center.z);
+            measurement.at<track_t>(3) = static_cast<track_t>(rect.size.width);
+            measurement.at<track_t>(4) = static_cast<track_t>(rect.size.height);
+            measurement.at<track_t>(5) = static_cast<track_t>(rect.size.length);
+            measurement.at<track_t>(6) = static_cast<track_t>(rect.yaw);
+        }
+        // Correction
+        cv::Mat estimated;
+        switch (m_type)
+        {
+        case tracking::KalmanLinear:
+        {
+            estimated = m_linearKalman.correct(measurement);
+
+            m_lastRect3DResult.center.x = estimated.at<track_t>(0);   //update using measurements
+            m_lastRect3DResult.center.y = estimated.at<track_t>(1);
+            m_lastRect3DResult.center.z = estimated.at<track_t>(2);
+            m_lastRect3DResult.size.width = estimated.at<track_t>(3);
+            m_lastRect3DResult.size.height = estimated.at<track_t>(4);
+            m_lastRect3DResult.size.length = estimated.at<track_t>(5);
+            m_lastRect3DResult.yaw = estimated.at<track_t>(6);
+
             // Inertia correction
 			if (!m_useAcceleration)
 			{
-				track_t currDist = sqrtf(sqr(estimated.at<track_t>(0) - rect.x) + sqr(estimated.at<track_t>(1) - rect.y) + sqr(estimated.at<track_t>(2) - rect.width) + sqr(estimated.at<track_t>(3) - rect.height));
+                track_t currDist = sqrtf(sqr(estimated.at<track_t>(0) - rect.center.x) + sqr(estimated.at<track_t>(1) - rect.center.y)+ sqr(estimated.at<track_t>(2) - rect.center.z) + sqr(estimated.at<track_t>(3) - rect.size.width) + sqr(estimated.at<track_t>(4) - rect.size.height) + sqr(estimated.at<track_t>(5) - rect.size.length) + sqr(estimated.at<track_t>(6) - rect.yaw));
 				if (currDist > m_lastDist)
 				{
 					m_deltaTime = std::min(m_deltaTime + m_deltaStep, m_deltaTimeMax);
@@ -838,46 +809,32 @@ cv::Rect TKalmanFilter::Update(cv::Rect rect, bool dataCorrect)
 				}
 				m_lastDist = currDist;
 
-				m_linearKalman.transitionMatrix.at<track_t>(0, 4) = m_deltaTime;
-				m_linearKalman.transitionMatrix.at<track_t>(1, 5) = m_deltaTime;
-				m_linearKalman.transitionMatrix.at<track_t>(2, 6) = m_deltaTime;
-				m_linearKalman.transitionMatrix.at<track_t>(3, 7) = m_deltaTime;
+                m_linearKalman.transitionMatrix.at<track_t>(0, 7) = m_deltaTime;
+                m_linearKalman.transitionMatrix.at<track_t>(1, 8) = m_deltaTime;
+                m_linearKalman.transitionMatrix.at<track_t>(2, 9) = m_deltaTime;
+                m_linearKalman.transitionMatrix.at<track_t>(3, 10) = m_deltaTime;
+                m_linearKalman.transitionMatrix.at<track_t>(4, 11) = m_deltaTime;
+                m_linearKalman.transitionMatrix.at<track_t>(5, 12) = m_deltaTime;
+                m_linearKalman.transitionMatrix.at<track_t>(6, 13) = m_deltaTime;
 			}
             break;
         }
-
-        case tracking::KalmanUnscented:
-        case tracking::KalmanAugmentedUnscented:
-#ifdef USE_OCV_UKF
-            estimated = m_uncsentedKalman->correct(measurement);
-
-            m_lastRectResult.x = estimated.at<track_t>(0);   //update using measurements
-            m_lastRectResult.y = estimated.at<track_t>(1);
-            m_lastRectResult.width = estimated.at<track_t>(6);
-            m_lastRectResult.height = estimated.at<track_t>(7);
-#else
-            estimated = m_linearKalman.correct(measurement);
-
-            m_lastRectResult.x = estimated.at<track_t>(0);   //update using measurements
-            m_lastRectResult.y = estimated.at<track_t>(1);
-            m_lastRectResult.width = estimated.at<track_t>(2);
-            m_lastRectResult.height = estimated.at<track_t>(3);
-            std::cerr << "UnscentedKalmanFilter was disabled in CMAKE! Set KalmanLinear in constructor." << std::endl;
-#endif
-            break;
         }
     }
     else
     {
         if (dataCorrect)
         {
-            m_lastRectResult.x = static_cast<track_t>(rect.x);
-            m_lastRectResult.y = static_cast<track_t>(rect.y);
-            m_lastRectResult.width = static_cast<track_t>(rect.width);
-            m_lastRectResult.height = static_cast<track_t>(rect.height);
+            m_lastRect3DResult.center.x = static_cast<track_t>(rect.center.x);
+            m_lastRect3DResult.center.y = static_cast<track_t>(rect.center.y);
+            m_lastRect3DResult.center.z = static_cast<track_t>(rect.center.z);
+            m_lastRect3DResult.size.width = static_cast<track_t>(rect.size.width);
+            m_lastRect3DResult.size.height = static_cast<track_t>(rect.size.height);
+            m_lastRect3DResult.size.length = static_cast<track_t>(rect.size.length);
+            m_lastRect3DResult.yaw = static_cast<track_t>(rect.yaw);
         }
     }
-    return cv::Rect(static_cast<int>(m_lastRectResult.x), static_cast<int>(m_lastRectResult.y), static_cast<int>(m_lastRectResult.width), static_cast<int>(m_lastRectResult.height));
+    return m_lastRect3DResult;
 }
 
 //---------------------------------------------------------------------------
@@ -894,27 +851,22 @@ cv::Vec<track_t, 2> TKalmanFilter::GetVelocity() const
             {
                 int indX = 2;
                 int indY = 3;
-                if (m_linearKalman.statePre.rows > 4)
+                if (m_linearKalman.statePre.rows > 5)
                 {
                     indX = 4;
                     indY = 5;
+                    if (m_linearKalman.statePre.rows > 8)
+                    {
+                        indX = 7;
+                        indY = 8;
+                    }
+                    res[0] = m_linearKalman.statePre.at<track_t>(indX);
+                    res[1] = m_linearKalman.statePre.at<track_t>(indY);
                 }
-                res[0] = m_linearKalman.statePre.at<track_t>(indX);
-                res[1] = m_linearKalman.statePre.at<track_t>(indY);
             }
             break;
         }
 
-        case tracking::KalmanUnscented:
-        case tracking::KalmanAugmentedUnscented:
-#ifdef USE_OCV_UKF
-            cv::Mat state = m_uncsentedKalman->getState();
-            res[0] = state.at<track_t>(2);
-            res[1] = state.at<track_t>(3);
-#else
-            std::cerr << "UnscentedKalmanFilter was disabled in CMAKE! Set KalmanLinear in constructor." << std::endl;
-#endif
-            break;
         }
     }
     return res;

src/fusion/lidar_radar_fusion_cnn/Tracker/Kalman.h

@@ -5,11 +5,6 @@
 
 #include <opencv2/opencv.hpp>
 
-#ifdef USE_OCV_UKF
-#include <opencv2/tracking.hpp>
-#include <opencv2/tracking/kalman_filters.hpp>
-#endif
-
 ///
 /// \brief The TKalmanFilter class
 /// http://www.morethantechnical.com/2011/06/17/simple-kalman-filter-for-tracking-using-opencv-2-2-w-code/
@@ -20,29 +15,32 @@ public:
     TKalmanFilter(tracking::KalmanType type, bool useAcceleration, track_t deltaTime, track_t accelNoiseMag);
     ~TKalmanFilter() = default;
 
-    Point_t GetPointPrediction();
-    Point_t Update(Point_t pt, bool dataCorrect);
+    cv::Point3f GetPointPrediction();
+    cv::Point3f Update(cv::Point3f pt, bool dataCorrect);
 
     cv::Rect GetRectPrediction();
     cv::Rect Update(cv::Rect rect, bool dataCorrect);
 
+    Rect3D GetRect3DPrediction();
+    Rect3D Update(Rect3D rect, bool dataCorrect);
+
 	cv::Vec<track_t, 2> GetVelocity() const;
 
 private:
     tracking::KalmanType m_type = tracking::KalmanLinear;
     cv::KalmanFilter m_linearKalman;
-#ifdef USE_OCV_UKF
-    cv::Ptr<cv::tracking::UnscentedKalmanFilter> m_uncsentedKalman;
-#endif
-
-    std::deque<Point_t> m_initialPoints;
-    std::deque<cv::Rect> m_initialRects;
     static const size_t MIN_INIT_VALS = 4;
+    std::deque<cv::Point3f> m_initialPoints;
+    cv::Point3f m_lastPointResult;
 
-    Point_t m_lastPointResult;
+    std::deque<cv::Rect> m_initialRects;
     cv::Rect_<track_t> m_lastRectResult;
     cv::Rect_<track_t> m_lastRect;
 
+    std::deque<Rect3D> m_initialRect3Ds;
+    Rect3D m_lastRect3DResult;
+    cv::Rect_<track_t> m_lastRect3D;
+
     bool m_initialized = false;
     track_t m_deltaTime = 0.2f;
     track_t m_deltaTimeMin = 0.2f;
@@ -54,20 +52,15 @@ private:
 	bool m_useAcceleration = false; // If set true then will be used motion model x(t) = x0 + v0 * t + a * t^2 / 2
 
 	// Constant velocity model
-    void CreateLinear(Point_t xy0, Point_t xyv0);
+    void CreateLinear(cv::Point3f xy0, cv::Point3f xyv0);
     void CreateLinear(cv::Rect_<track_t> rect0, Point_t rectv0);
-	
+    void CreateLinear3D(Rect3D rect0, cv::Point3f rectv0);
+
 	// Constant acceleration model
 	// https://www.mathworks.com/help/driving/ug/linear-kalman-filters.html
-	void CreateLinearAcceleration(Point_t xy0, Point_t xyv0);
-	void CreateLinearAcceleration(cv::Rect_<track_t> rect0, Point_t rectv0);
-
-#ifdef USE_OCV_UKF
-    void CreateUnscented(Point_t xy0, Point_t xyv0);
-    void CreateUnscented(cv::Rect_<track_t> rect0, Point_t rectv0);
-    void CreateAugmentedUnscented(Point_t xy0, Point_t xyv0);
-    void CreateAugmentedUnscented(cv::Rect_<track_t> rect0, Point_t rectv0);
-#endif
+    void CreateLinearAcceleration(cv::Point3f xy0, cv::Point3f xyv0);
+    void CreateLinearAcceleration(cv::Rect_<track_t> rect0, Point_t rectv0);
+    void CreateLinearAcceleration3D(Rect3D rect0, cv::Point3f rectv0);
 };
 
 //---------------------------------------------------------------------------

src/fusion/lidar_radar_fusion_cnn/Tracker/ShortPathCalculator.h

@@ -1,86 +0,0 @@
-#pragma once
-#include "fusionobjectarray.pb.h"
-#include "fusionobject.pb.h"
-#include "Ctracker.h"
-
-#include <iostream>
-#include <vector>
-bool InitTracker(CTracker& tracker)
-{
-    TrackerSettings settings;
-    settings.SetDistance(tracking::DistCenters);
-    settings.m_kalmanType = tracking::KalmanLinear;
-    settings.m_filterGoal = tracking::FilterCenter;
-    settings.m_lostTrackType = tracking::TrackNone;
-    settings.m_matchType = tracking::MatchHungrian;
-    settings.m_dt = 0.2f;
-    settings.m_accelNoiseMag = 0.5f;
-    settings.m_distThres = 0.8f;
-    settings.m_minAreaRadiusPix = -1.f;//frame.rows / 20.f;
-    settings.m_maximumAllowedSkippedFrames = 30;
-    settings.m_maxTraceLength = 60;
-
-    tracker.setSettings(settings);
-
-    return true;
-}
-
-// ----------------------------------------------------------------------
-void Tracking(iv::fusion::fusionobjectarray& fusionobjvec, CTracker& tracker)
-{
-    //  std::vector<Point_t> pts;
-    regions_t regions;
-    Point_t pointXY;
-    for(int i = 0;i<fusionobjvec.obj_size();i++)
-    {
-        pointXY.x = fusionobjvec.obj(i).centroid().x();
-        pointXY.y = fusionobjvec.obj(i).centroid().y();
-        int w = fabs(fusionobjvec.obj(i).dimensions().x());
-        int h = fabs(fusionobjvec.obj(i).dimensions().z());
-        cv::RotatedRect rrect = cv::RotatedRect(pointXY,cv::Size2f(w>0?w:w+0.0001,h>0?h:h+0.0001),0);
-        CRegion region = CRegion(rrect,fusionobjvec.obj(i).type_name(),fusionobjvec.obj(i).prob());
-        regions.push_back(region);
-        //std::cout<<"old x,y:"<<pointXY.x<<","<<pointXY.y<<std::endl;
-    }
-
-    tracker.Update(regions, cv::UMat(), 30);
-
-    auto tracks = tracker.GetTracks();
-    // std::cout<<fusionobjvec.obj_size()<<","<<tracks.size()<<std::endl;
-    for (size_t i = 0; i < tracks.size(); i++)
-    {
-        const auto& track = tracks[i];
-        int obj_id = track.m_regionID;
-        if(obj_id != -1)
-        {
-            iv::fusion::fusionobject &fusion_object = (iv::fusion::fusionobject &)fusionobjvec.obj(obj_id);
-            fusion_object.set_id(track.m_ID);
-            // std::cout<<"i:"<<fusion_object.id()<<std::endl;
-            // std::cout<<"update x,y:"<<track.m_trace[track.m_trace.size()-1].x<<","<<track.m_trace[track.m_trace.size()-1].y<<std::endl;
-            iv::fusion::PointXYZ centroid;
-            iv::fusion::PointXYZ *centerpoint;
-            iv::fusion::PointXYZ point_forecast;
-            centroid.set_x(track.m_trace[track.m_trace.size()-1].x);
-            centroid.set_y(track.m_trace[track.m_trace.size()-1].y);
-            centerpoint=fusion_object.mutable_centroid();
-            centerpoint->CopyFrom(centroid);
-            for(int j=0;j<track.m_trace.size();j++)
-            {
-                point_forecast.set_x(track.m_trace[j].x);
-                point_forecast.set_y(track.m_trace[j].y);
-                iv::fusion::PointXYZ *p = fusion_object.add_point_forecast();
-                p->CopyFrom(point_forecast);
-            }
-        }
-
-    }
-//    for (size_t i = 0; i < fusionobjvec.obj_size(); i++)
-//    {
-//        iv::fusion::fusionobject fusion_object;
-//        fusion_object = fusionobjvec.obj(i);
-//        std::cout<<"forecast size:"<<fusion_object.point_forecast_size()<<std::endl;
-//    }
-    regions.clear();
-    tracks.clear();
-}
-

src/fusion/lidar_radar_fusion_cnn/Tracker/VOTTracker.hpp

@@ -1,21 +0,0 @@
-#pragma once
-#include <opencv2/opencv.hpp>
-///
-/// \brief The VOTTracker class
-///
-class VOTTracker
-{
-public:
-    VOTTracker()
-    {
-
-    }
-    virtual ~VOTTracker()
-    {
-
-    }
-
-    virtual void Initialize(const cv::Mat &im, cv::Rect region) = 0;
-    virtual cv::RotatedRect Update(const cv::Mat &im, float& confidence) = 0;
-    virtual void Train(const cv::Mat &im, bool first) = 0;
-};

src/fusion/lidar_radar_fusion_cnn/Tracker/defines.h

@@ -16,7 +16,36 @@ typedef cv::Point_<track_t> Point_t;
 
 typedef std::vector<int> assignments_t;
 typedef std::vector<track_t> distMatrix_t;
-
+typedef struct Size3D{
+    Size3D()
+    {
+    }
+    Size3D(float w, float h, float l)
+        :
+          width(w),
+          height(h),
+          length(l)
+    {
+    }
+    float length;
+    float width;
+    float height;
+} Size3D;
+typedef struct Rect3D{
+    Rect3D()
+    {
+    }
+    Rect3D(cv::Point3f c, Size3D s, float y)
+        :
+          center(c),
+          size(s),
+          yaw(y)
+    {
+    }
+    cv::Point3f center;
+    Size3D size;
+    float yaw;
+} Rect3D;
 ///
 /// \brief config_t
 ///
@@ -29,57 +58,36 @@ class CRegion
 {
 public:
     CRegion()
-        : m_type(""), m_confidence(-1)
-    {
-    }
-
-    CRegion(const cv::Rect& rect)
-        : m_brect(rect)
-    {
-        B2RRect();
-    }
-
-    CRegion(const cv::RotatedRect& rrect)
-        : m_rrect(rrect)
+        : m_type(-1), m_type_name(""), m_confidence(-1)
     {
-        R2BRect();
     }
 
-    CRegion(const cv::Rect& rect, const std::string& type, float confidence)
-        : m_brect(rect), m_type(type), m_confidence(confidence)
+    CRegion(const Rect3D& rect, const int& type, float confidence)
+        : m_rect(rect), m_type(type), m_confidence(confidence)
     {
-        B2RRect();
+        RBRect();
     }
 
-    CRegion(const cv::RotatedRect& rrect, const std::string& type, float confidence)
-        : m_rrect(rrect), m_type(type), m_confidence(confidence)
+    CRegion(const Rect3D& rect, const std::string& type, float confidence)
+        : m_rect(rect), m_type_name(type), m_confidence(confidence)
     {
-        R2BRect();
+        RBRect();
     }
+    Rect3D m_rect;
     cv::RotatedRect m_rrect;
     cv::Rect m_brect;
-
-    std::string m_type;
+    int m_type = -1;
+    std::string m_type_name = "";
     float m_confidence = -1;
-	mutable cv::Mat m_hist;
-
 private:
-    ///
-    /// \brief R2BRect
-    /// \return
-    ///
-    cv::Rect R2BRect()
-    {
-        m_brect = m_rrect.boundingRect();
-        return m_brect;
-    }
     ///
     /// \brief B2RRect
     /// \return
     ///
-    cv::RotatedRect B2RRect()
+    cv::RotatedRect RBRect()
     {
-        m_rrect = cv::RotatedRect(m_brect.tl(), cv::Point2f(static_cast<float>(m_brect.x + m_brect.width), static_cast<float>(m_brect.y)), m_brect.br());
+        m_rrect = cv::RotatedRect(Point_t(m_rect.center.x,m_rect.center.y), cv::Size(m_rect.size.width,m_rect.size.height), 0);
+        m_brect = cv::Rect(m_rect.center.x-m_rect.size.width/2,m_rect.center.y-m_rect.size.height/2, m_rect.size.width,m_rect.size.height);
         return m_rrect;
     }
 };
@@ -91,26 +99,6 @@ typedef std::vector<CRegion> regions_t;
 ///
 namespace tracking
 {
-///
-/// \brief The Detectors enum
-///
-enum Detectors
-{
-    Motion_VIBE,
-    Motion_MOG,
-    Motion_GMG,
-    Motion_CNT,
-    Motion_SuBSENSE,
-    Motion_LOBSTER,
-    Motion_MOG2,
-    Face_HAAR,
-    Pedestrian_HOG,
-    Pedestrian_C4,
-    SSD_MobileNet,
-    Yolo_OCV,
-	Yolo_Darknet,
-	Yolo_TensorRT
-};
 
 ///
 /// \brief The DistType enum
@@ -119,8 +107,7 @@ enum DistType
 {
     DistCenters,   // Euclidean distance between centers, pixels
     DistRects,     // Euclidean distance between bounding rectangles, pixels
-    DistJaccard,   // Intersection over Union, IoU, [0, 1]
-	DistHist,      // Bhatacharia distance between histograms, [0, 1]
+    DistRect3Ds,     // Euclidean distance between bounding rectangles, pixels
 	DistsCount
 };
 
@@ -129,8 +116,9 @@ enum DistType
 ///
 enum FilterGoal
 {
-    FilterCenter,
-    FilterRect
+    FilterCenter, // x,y
+    FilterRect,   // x,y,w,h
+    FilterRect3D  // x,y,z,w,h,l,yaw
 };
 
 ///
@@ -138,9 +126,7 @@ enum FilterGoal
 ///
 enum KalmanType
 {
-    KalmanLinear,
-    KalmanUnscented,
-    KalmanAugmentedUnscented
+    KalmanLinear
 };
 
 ///
@@ -148,8 +134,7 @@ enum KalmanType
 ///
 enum MatchType
 {
-    MatchHungrian/*,
-    //MatchBipart*/
+    MatchHungrian
 };
 
 ///
@@ -157,15 +142,6 @@ enum MatchType
 ///
 enum LostTrackType
 {
-    TrackNone,
-    TrackKCF,
-    TrackMIL,
-    TrackMedianFlow,
-    TrackGOTURN,
-    TrackMOSSE,
-    TrackCSRT/*,
-    TrackDAT,
-    TrackSTAPLE,
-    TrackLDES*/
+    TrackNone
 };
 }

src/fusion/lidar_radar_fusion_cnn/Tracker/track.cpp

@@ -12,15 +12,14 @@
 /// \param filterObjectSize
 /// \param externalTrackerForLost
 ///
-CTrack::CTrack(
-        const CRegion& region,
+CTrack::CTrack(const CRegion& region,
         tracking::KalmanType kalmanType,
         track_t deltaTime,
         track_t accelNoiseMag,
         bool useAcceleration,
         size_t trackID,
         size_t regionID,
-        bool filterObjectSize,
+        tracking::FilterGoal filterGoal,
         tracking::LostTrackType externalTrackerForLost
         )
     :
@@ -28,15 +27,18 @@ CTrack::CTrack(
       m_regionID(regionID),
       m_skippedFrames(0),
       m_lastRegion(region),
-      m_predictionPoint(region.m_rrect.center),
+      m_predictionPoint(region.m_rect.center),
       m_predictionRect(region.m_rrect),
+      m_predictionRect3D(region.m_rect),
       m_kalman(kalmanType, useAcceleration, deltaTime, accelNoiseMag),
-      m_filterObjectSize(filterObjectSize),
+      m_filterGoal(filterGoal),
       m_outOfTheFrame(false),
       m_externalTrackerForLost(externalTrackerForLost)
 {
-    if (filterObjectSize)
+    if (filterGoal == tracking::FilterRect)
         m_kalman.Update(region.m_brect, true);
+    else if(filterGoal == tracking::FilterRect3D)
+        m_kalman.Update(region.m_rect, true);
     else
         m_kalman.Update(m_predictionPoint, true);
 
@@ -50,10 +52,9 @@ CTrack::CTrack(
 ///
 track_t CTrack::CalcDistCenter(const CRegion& reg) const
 {
-    Point_t diff = m_predictionPoint - reg.m_rrect.center;
+    cv::Point3f diff = m_predictionPoint - reg.m_rect.center;
     return sqrtf(sqr(diff.x) + sqr(diff.y));
 }
-
 ///
 /// \brief CTrack::CalcDistRect
 /// \param reg
@@ -75,59 +76,28 @@ track_t CTrack::CalcDistRect(const CRegion& reg) const
     }
     return sqrtf(dist);
 }
-
-///
-/// \brief CTrack::CalcDistJaccard
-/// \param reg
-/// \return
-///
-track_t CTrack::CalcDistJaccard(const CRegion& reg) const
-{
-    track_t intArea = static_cast<track_t>((reg.m_brect & m_lastRegion.m_brect).area());
-    track_t unionArea = static_cast<track_t>(reg.m_brect.area() + m_lastRegion.m_brect.area() - intArea);
-
-    return 1 - intArea / unionArea;
-}
-
 ///
-/// \brief CTrack::CalcDistHist
+/// \brief CTrack::CalcDistRect3D
 /// \param reg
 /// \return
 ///
-track_t CTrack::CalcDistHist(const CRegion& reg, cv::UMat currFrame) const
+track_t CTrack::CalcDistRect3D(const CRegion& reg) const
 {
-    track_t res = 1;
-
-    if (reg.m_hist.empty())
-    {
-        int bins = 64;
-        std::vector<int> histSize;
-        std::vector<float> ranges;
-        std::vector<int> channels;
-
-        for (int i = 0, stop = currFrame.channels(); i < stop; ++i)
-        {
-            histSize.push_back(bins);
-            ranges.push_back(0);
-            ranges.push_back(255);
-            channels.push_back(i);
-        }
+    std::array<track_t, 7> diff;
+    diff[0] = reg.m_rect.center.x - m_lastRegion.m_rect.center.x;
+    diff[1] = reg.m_rect.center.y - m_lastRegion.m_rect.center.y;
+    diff[2] = 0;//reg.m_rect.center.z - m_lastRegion.m_rect.center.z;
+    diff[3] = static_cast<track_t>(m_lastRegion.m_rect.size.width - reg.m_rect.size.width);
+    diff[4] = static_cast<track_t>(m_lastRegion.m_rect.size.height - reg.m_rect.size.height);
+    diff[5] = static_cast<track_t>(m_lastRegion.m_rect.size.length - reg.m_rect.size.length);
+    diff[6] = 0;//static_cast<track_t>(m_lastRegion.m_rect.yaw - reg.m_rect.yaw);
 
-        std::vector<cv::UMat> regROI = { currFrame(reg.m_brect) };
-        cv::calcHist(regROI, channels, cv::Mat(), reg.m_hist, histSize, ranges, false);
-        cv::normalize(reg.m_hist, reg.m_hist, 0, 1, cv::NORM_MINMAX, -1, cv::Mat());
-    }
-    if (!reg.m_hist.empty() && !m_lastRegion.m_hist.empty())
+    track_t dist = 0;
+    for (size_t i = 0; i < diff.size(); ++i)
     {
-#if (((CV_VERSION_MAJOR == 4) && (CV_VERSION_MINOR < 1)) || (CV_VERSION_MAJOR == 3))
-        res = static_cast<track_t>(cv::compareHist(reg.m_hist, m_lastRegion.m_hist, CV_COMP_BHATTACHARYYA));
-        //res = 1.f - static_cast<track_t>(cv::compareHist(reg.m_hist, m_lastRegion.m_hist, CV_COMP_CORREL));
-#else
-        res = static_cast<track_t>(cv::compareHist(reg.m_hist, m_lastRegion.m_hist, cv::HISTCMP_BHATTACHARYYA));
-#endif
+        dist += sqr(diff[i]);
     }
-
-    return res;
+    return sqrtf(dist);
 }
 
 ///
@@ -148,17 +118,19 @@ void CTrack::Update(
         int trajLen
         )
 {
-    if (m_filterObjectSize) // Kalman filter for object coordinates and size
+    if (m_filterGoal == tracking::FilterRect) // Kalman filter for object coordinates and size
         RectUpdate(region, dataCorrect, prevFrame, currFrame);
+    if (m_filterGoal == tracking::FilterRect3D) // Kalman filter for object coordinates and size
+        Rect3DUpdate(region, dataCorrect, prevFrame, currFrame);
     else // Kalman filter only for object center
-        PointUpdate(region.m_rrect.center, region.m_rrect.size, dataCorrect, currFrame.size());
+        PointUpdate(region.m_rect.center, region.m_rrect.size, dataCorrect, currFrame.size());
 
     if (dataCorrect)
     {
         //std::cout << m_lastRegion.m_brect << " - " << region.m_brect << std::endl;
 
         m_lastRegion = region;
-        m_trace.push_back(m_predictionPoint, region.m_rrect.center);
+        m_trace.push_back(m_predictionPoint, region.m_rect.center);
 
         CheckStatic(trajLen, currFrame, region);
     }
@@ -200,33 +172,34 @@ bool CTrack::IsOutOfTheFrame() const
 }
 
 ///
-cv::RotatedRect CTrack::CalcPredictionEllipse(cv::Size_<track_t> minRadius) const
-{
-    // Move ellipse to velocity
-    auto velocity = m_kalman.GetVelocity();
-    Point_t d(3.f * velocity[0], 3.f * velocity[1]);
+//cv::RotatedRect CTrack::CalcPredictionEllipse(cv::Size_<track_t> minRadius) const
+//{
+//    // Move ellipse to velocity
+//    auto velocity = m_kalman.GetVelocity();
+//    Point_t d(3.f * velocity[0], 3.f * velocity[1]);
 
-    cv::RotatedRect rrect(m_predictionPoint, cv::Size2f(std::max(minRadius.width, float(fabs(d.x))), std::max(minRadius.height, float(fabs(d.y)))), 0);
-    if (fabs(d.x) + fabs(d.y) > 4) // pix
-    {
-        if (fabs(d.x) > 0.0001f)
-        {
-            track_t l = std::min(rrect.size.width, rrect.size.height) / 3;
+//    cv::RotatedRect rrect(m_predictionPoint, cv::Size2f(std::max(minRadius.width, fabs(d.x)), std::max(minRadius.height, fabs(d.y))), 0);
 
-            track_t p2_l = sqrtf(sqr(d.x) + sqr(d.y));
-            rrect.center.x = l * d.x / p2_l + m_predictionPoint.x;
-            rrect.center.y = l * d.y / p2_l + m_predictionPoint.y;
+//    if (fabs(d.x) + fabs(d.y) > 4) // pix
+//    {
+//        if (fabs(d.x) > 0.0001f)
+//        {
+//            track_t l = std::min(rrect.size.width, rrect.size.height) / 3;
 
-            rrect.angle = atanf(d.y / d.x);
-        }
-        else
-        {
-            rrect.center.y += d.y / 3;
-            rrect.angle = static_cast<float>(CV_PI / 2.);
-        }
-    }
-    return rrect;
-}
+//            track_t p2_l = sqrtf(sqr(d.x) + sqr(d.y));
+//            rrect.center.x = l * d.x / p2_l + m_predictionPoint.x;
+//            rrect.center.y = l * d.y / p2_l + m_predictionPoint.y;
+
+//            rrect.angle = atanf(d.y / d.x);
+//        }
+//        else
+//        {
+//            rrect.center.y += d.y / 3;
+//            rrect.angle = static_cast<float>(CV_PI / 2.);
+//        }
+//    }
+//    return rrect;
+//}
 
 ///
 /// \brief CTrack::IsInsideArea
@@ -252,10 +225,10 @@ track_t CTrack::IsInsideArea(const Point_t& pt, const cv::RotatedRect& rrect) co
 ///
 track_t CTrack::WidthDist(const CRegion& reg) const
 {
-    if (m_lastRegion.m_rrect.size.width < reg.m_rrect.size.width)
-        return m_lastRegion.m_rrect.size.width / reg.m_rrect.size.width;
+    if (m_lastRegion.m_rect.size.width < reg.m_rect.size.width)
+        return m_lastRegion.m_rect.size.width / reg.m_rect.size.width;
     else
-        return reg.m_rrect.size.width / m_lastRegion.m_rrect.size.width;
+        return reg.m_rect.size.width / m_lastRegion.m_rect.size.width;
 }
 
 ///
@@ -265,10 +238,10 @@ track_t CTrack::WidthDist(const CRegion& reg) const
 ///
 track_t CTrack::HeightDist(const CRegion& reg) const
 {
-    if (m_lastRegion.m_rrect.size.height < reg.m_rrect.size.height)
-        return m_lastRegion.m_rrect.size.height / reg.m_rrect.size.height;
+    if (m_lastRegion.m_rect.size.height < reg.m_rect.size.height)
+        return m_lastRegion.m_rect.size.height / reg.m_rect.size.height;
     else
-        return reg.m_rrect.size.height / m_lastRegion.m_rrect.size.height;
+        return reg.m_rect.size.height / m_lastRegion.m_rect.size.height;
 }
 
 ///
@@ -298,27 +271,7 @@ bool CTrack::CheckStatic(int trajLen, cv::UMat currFrame, const CRegion& region)
             if (!m_isStatic)
             {
                 m_staticFrame = currFrame.clone();
-                m_staticRect = region.m_brect;
-#if 0
-#ifndef SILENT_WORK
-                cv::namedWindow("m_staticFrame", cv::WINDOW_NORMAL);
-                cv::Mat img = m_staticFrame.getMat(cv::ACCESS_READ).clone();
-                cv::rectangle(img, m_staticRect, cv::Scalar(255, 0, 255), 1);
-                for (size_t i = m_trace.size() - trajLen; i < m_trace.size() - 1; ++i)
-                {
-                    cv::line(img, m_trace[i], m_trace[i + 1], cv::Scalar(0, 0, 0), 1, cv::LINE_8);
-                }
-                std::string label = "(" + std::to_string(kx) + ", "  + std::to_string(ky) + ") = " + std::to_string(speed);
-                cv::line(img,
-                         cv::Point(cvRound(bx), cvRound(by)),
-                         cv::Point(cvRound(kx * trajLen + bx), cvRound(ky * trajLen + by)),
-                         cv::Scalar(0, 0, 0), 1, cv::LINE_8);
-                cv::putText(img, label, m_staticRect.tl(), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 0));
-                cv::imshow("m_staticFrame", img);
-                std::cout << "m_staticRect = " << m_staticRect << std::endl;
-                cv::waitKey(1);
-#endif
-#endif
+                m_staticRect = region.m_rect;
             }
 
             ++m_staticFrames;
@@ -339,18 +292,16 @@ bool CTrack::CheckStatic(int trajLen, cv::UMat currFrame, const CRegion& region)
 /// \brief GetLastRect
 /// \return
 ///
-cv::RotatedRect CTrack::GetLastRect() const
+Rect3D CTrack::GetLastRect() const
 {
-    if (m_filterObjectSize)
-    {
-        return m_predictionRect;
-    }
+    if (m_filterGoal == tracking::FilterRect)
+        return Rect3D(cv::Point3f(m_predictionRect.center.x,m_predictionRect.center.y,0),Size3D(m_predictionRect.boundingRect().width,m_predictionRect.boundingRect().height,0),0);
+    else if(m_filterGoal == tracking::FilterRect3D)
+        return m_predictionRect3D;
     else
-    {
-        return cv::RotatedRect(cv::Point2f(m_predictionPoint.x, m_predictionPoint.y), m_predictionRect.size, m_predictionRect.angle);
-    }
-}
+        return Rect3D(cv::Point3f(m_predictionPoint.x, m_predictionPoint.y,0),Size3D(0,0,0),0);
 
+}
 ///
 /// \brief CTrack::LastRegion
 /// \return
@@ -366,8 +317,7 @@ const CRegion& CTrack::LastRegion() const
 ///
 TrackingObject CTrack::ConstructObject() const
 {
-    return TrackingObject(GetLastRect(), m_trackID, m_regionID, m_trace, IsStatic(), IsOutOfTheFrame(),
-                          m_lastRegion.m_type, m_lastRegion.m_confidence, m_kalman.GetVelocity());
+    return TrackingObject(GetLastRect(), m_lastRegion, m_trackID, m_regionID, m_trace, IsStatic(), IsOutOfTheFrame(), m_kalman.GetVelocity(), m_detectedFrames);
 }
 
 ///
@@ -387,7 +337,14 @@ size_t& CTrack::SkippedFrames()
 {
     return m_skippedFrames;
 }
-
+///
+/// \brief CTrack::DetectedFrames
+/// \return
+///
+size_t& CTrack::DetectedFrames()
+{
+    return m_detectedFrames;
+}
 ///
 /// \brief RectUpdate
 /// \param region
@@ -446,174 +403,6 @@ void CTrack::RectUpdate(
     {
     case tracking::TrackNone:
         break;
-
-    case tracking::TrackKCF:
-    case tracking::TrackMIL:
-    case tracking::TrackMedianFlow:
-    case tracking::TrackGOTURN:
-    case tracking::TrackMOSSE:
-    case tracking::TrackCSRT:
-#ifdef USE_OCV_KCF
-        if (!dataCorrect)
-        {
-            cv::Rect brect = m_predictionRect.boundingRect();
-
-            cv::Size roiSize(std::max(2 * brect.width, currFrame.cols / 4), std::max(2 * brect.height, currFrame.rows / 4));
-            if (roiSize.width > currFrame.cols)
-                roiSize.width = currFrame.cols;
-
-            if (roiSize.height > currFrame.rows)
-                roiSize.height = currFrame.rows;
-
-            cv::Point roiTL(brect.x + brect.width / 2 - roiSize.width / 2, brect.y + brect.height / 2 - roiSize.height / 2);
-            cv::Rect roiRect(roiTL, roiSize);
-            Clamp(roiRect.x, roiRect.width, currFrame.cols);
-            Clamp(roiRect.y, roiRect.height, currFrame.rows);
-
-            bool inited = false;
-            if (!m_tracker || m_tracker.empty())
-            {
-                CreateExternalTracker(currFrame.channels());
-
-                cv::Rect2d lastRect(brect.x - roiRect.x, brect.y - roiRect.y, brect.width, brect.height);
-                if (m_staticFrame.empty())
-                {
-                    int dx = 1;//m_predictionRect.width / 8;
-                    int dy = 1;//m_predictionRect.height / 8;
-                    lastRect = cv::Rect2d(brect.x - roiRect.x - dx, brect.y - roiRect.y - dy, brect.width + 2 * dx, brect.height + 2 * dy);
-                }
-                else
-                {
-                    lastRect = cv::Rect2d(m_staticRect.x - roiRect.x, m_staticRect.y - roiRect.y, m_staticRect.width, m_staticRect.height);
-                }
-
-                if (lastRect.x >= 0 &&
-                        lastRect.y >= 0 &&
-                        lastRect.x + lastRect.width < roiRect.width &&
-                        lastRect.y + lastRect.height < roiRect.height &&
-                        lastRect.area() > 0)
-                {
-                    if (m_staticFrame.empty())
-                        m_tracker->init(cv::UMat(prevFrame, roiRect), lastRect);
-                    else
-                        m_tracker->init(cv::UMat(m_staticFrame, roiRect), lastRect);
-#if 0
-#ifndef SILENT_WORK
-                    cv::Mat tmp = cv::UMat(prevFrame, roiRect).getMat(cv::ACCESS_READ).clone();
-                    cv::rectangle(tmp, lastRect, cv::Scalar(255, 255, 255), 2);
-                    cv::imshow("init", tmp);
-#endif
-#endif
-
-                    inited = true;
-                    m_outOfTheFrame = false;
-                }
-                else
-                {
-                    m_tracker.release();
-                    m_outOfTheFrame = true;
-                }
-            }
-            cv::Rect2d newRect;
-            if (!inited && !m_tracker.empty() && m_tracker->update(cv::UMat(currFrame, roiRect), newRect))
-            {
-#if 0
-#ifndef SILENT_WORK
-                cv::Mat tmp2 = cv::UMat(currFrame, roiRect).getMat(cv::ACCESS_READ).clone();
-                cv::rectangle(tmp2, newRect, cv::Scalar(255, 255, 255), 2);
-                cv::imshow("track", tmp2);
-#endif
-#endif
-
-                cv::Rect prect(cvRound(newRect.x) + roiRect.x, cvRound(newRect.y) + roiRect.y, cvRound(newRect.width), cvRound(newRect.height));
-
-                UpdateRRect(brect, m_kalman.Update(prect, true));
-
-                recalcPrediction = false;
-            }
-        }
-        else
-        {
-            if (m_tracker && !m_tracker.empty())
-                m_tracker.release();
-        }
-#else
-        std::cerr << "KCF tracker was disabled in CMAKE! Set lostTrackType = TrackNone in constructor." << std::endl;
-#endif
-        break;
-
-        //    case tracking::TrackDAT:
-        //    case tracking::TrackSTAPLE:
-        //    case tracking::TrackLDES:
-        //        if (!dataCorrect)
-        //        {
-        //            bool inited = false;
-        //            cv::Rect brect = m_predictionRect.boundingRect();
-        //            if (!m_VOTTracker)
-        //            {
-        //                CreateExternalTracker(currFrame.channels());
-
-        //                cv::Rect2d lastRect(brect.x, brect.y, brect.width, brect.height);
-        //                if (!m_staticFrame.empty())
-        //                    lastRect = cv::Rect2d(m_staticRect.x, m_staticRect.y, m_staticRect.width, m_staticRect.height);
-
-        //                if (lastRect.x >= 0 &&
-        //                        lastRect.y >= 0 &&
-        //                        lastRect.x + lastRect.width < prevFrame.cols &&
-        //                        lastRect.y + lastRect.height < prevFrame.rows &&
-        //                        lastRect.area() > 0)
-        //                {
-        //                    if (m_staticFrame.empty())
-        //                    {
-        //                        cv::Mat mat = prevFrame.getMat(cv::ACCESS_READ);
-        //                        m_VOTTracker->Initialize(mat, lastRect);
-        //                        m_VOTTracker->Train(mat, true);
-        //                    }
-        //                    else
-        //                    {
-        //                        cv::Mat mat = m_staticFrame.getMat(cv::ACCESS_READ);
-        //                        m_VOTTracker->Initialize(mat, lastRect);
-        //                        m_VOTTracker->Train(mat, true);
-        //                    }
-
-        //                    inited = true;
-        //                    m_outOfTheFrame = false;
-        //                }
-        //                else
-        //                {
-        //                    m_VOTTracker = nullptr;
-        //                    m_outOfTheFrame = true;
-        //                }
-        //            }
-        //            if (!inited && m_VOTTracker)
-        //            {
-        //                constexpr float confThresh = 0.3f;
-        //                cv::Mat mat = currFrame.getMat(cv::ACCESS_READ);
-        //                float confidence = 0;
-        //                cv::RotatedRect newRect = m_VOTTracker->Update(mat, confidence);
-        //                if (confidence > confThresh)
-        //                {
-        //                    m_VOTTracker->Train(mat, false);
-
-        //                    if (newRect.angle > 0.5f)
-        //                    {
-        //                        m_predictionRect = newRect;
-        //                        m_kalman.Update(newRect.boundingRect(), true);
-        //                    }
-        //                    else
-        //                    {
-        //                        UpdateRRect(brect, m_kalman.Update(newRect.boundingRect(), true));
-        //                    }
-        //                    recalcPrediction = false;
-        //                }
-        //            }
-        //        }
-        //        else
-        //        {
-        //            if (m_VOTTracker)
-        //                m_VOTTracker = nullptr;
-        //        }
-        //        break;
     }
 
     if (recalcPrediction)
@@ -622,191 +411,40 @@ void CTrack::RectUpdate(
     cv::Rect brect = m_predictionRect.boundingRect();
     int dx = Clamp(brect.x, brect.width, currFrame.cols);
     int dy = Clamp(brect.y, brect.height, currFrame.rows);
-#if 0
-    m_predictionRect.center.x += dx;
-    m_predictionRect.center.y += dy;
-#endif
+
     m_outOfTheFrame = (dx != 0) || (dy != 0) || (brect.width < 2) || (brect.height < 2);
 
-    m_predictionPoint = m_predictionRect.center;
+    m_predictionPoint = cv::Point3f(m_predictionRect.center.x,m_predictionRect.center.y,0);
 
     //std::cout << "brect = " << brect << ", dx = " << dx << ", dy = " << dy << ", outOfTheFrame = " << m_outOfTheFrame << ", predictionPoint = " << m_predictionPoint << std::endl;
 }
-
 ///
-/// \brief CreateExternalTracker
+/// \brief Rect3DUpdate
+/// \param region
+/// \param dataCorrect
+/// \param prevFrame
+/// \param currFrame
 ///
-void CTrack::CreateExternalTracker(int channels)
+void CTrack::Rect3DUpdate(
+        const CRegion& region,
+        bool dataCorrect,
+        cv::UMat prevFrame,
+        cv::UMat currFrame
+        )
 {
+    m_kalman.GetRect3DPrediction();
+
     switch (m_externalTrackerForLost)
     {
     case tracking::TrackNone:
-        if (m_VOTTracker)
-            m_VOTTracker = nullptr;
-
-#ifdef USE_OCV_KCF
-        if (m_tracker && !m_tracker.empty())
-            m_tracker.release();
-#endif
-        break;
-
-    case tracking::TrackKCF:
-#ifdef USE_OCV_KCF
-        if (!m_tracker || m_tracker.empty())
-        {
-            cv::TrackerKCF::Params params;
-            if (channels == 1)
-            {
-                params.compressed_size = 1;
-                params.desc_pca = cv::TrackerKCF::GRAY;
-                params.desc_npca = cv::TrackerKCF::GRAY;
-            }
-            else
-            {
-                params.compressed_size = 3;
-                params.desc_pca = cv::TrackerKCF::CN;
-                params.desc_npca = cv::TrackerKCF::CN;
-            }
-            params.resize = true;
-            params.detect_thresh = 0.7f;
-#if (((CV_VERSION_MAJOR == 3) && (CV_VERSION_MINOR >= 3)) || (CV_VERSION_MAJOR > 3))
-            m_tracker = cv::TrackerKCF::create(params);
-#else
-            m_tracker = cv::TrackerKCF::createTracker(params);
-#endif
-        }
-#endif
-        if (m_VOTTracker)
-            m_VOTTracker = nullptr;
-        break;
-
-    case tracking::TrackMIL:
-#ifdef USE_OCV_KCF
-        if (!m_tracker || m_tracker.empty())
-        {
-            cv::TrackerMIL::Params params;
-
-#if (((CV_VERSION_MAJOR == 3) && (CV_VERSION_MINOR >= 3)) || (CV_VERSION_MAJOR > 3))
-            m_tracker = cv::TrackerMIL::create(params);
-#else
-            m_tracker = cv::TrackerMIL::createTracker(params);
-#endif
-        }
-#endif
-        if (m_VOTTracker)
-            m_VOTTracker = nullptr;
-        break;
-
-    case tracking::TrackMedianFlow:
-#ifdef USE_OCV_KCF
-        if (!m_tracker || m_tracker.empty())
-        {
-            cv::TrackerMedianFlow::Params params;
-
-#if (((CV_VERSION_MAJOR == 3) && (CV_VERSION_MINOR >= 3)) || (CV_VERSION_MAJOR > 3))
-            m_tracker = cv::TrackerMedianFlow::create(params);
-#else
-            m_tracker = cv::TrackerMedianFlow::createTracker(params);
-#endif
-        }
-#endif
-        if (m_VOTTracker)
-            m_VOTTracker = nullptr;
-        break;
-
-    case tracking::TrackGOTURN:
-#ifdef USE_OCV_KCF
-        if (!m_tracker || m_tracker.empty())
-        {
-            cv::TrackerGOTURN::Params params;
-
-#if (((CV_VERSION_MAJOR == 3) && (CV_VERSION_MINOR >= 3)) || (CV_VERSION_MAJOR > 3))
-            m_tracker = cv::TrackerGOTURN::create(params);
-#else
-            m_tracker = cv::TrackerGOTURN::createTracker(params);
-#endif
-        }
-#endif
-        if (m_VOTTracker)
-            m_VOTTracker = nullptr;
         break;
+    }
 
-    case tracking::TrackMOSSE:
-#ifdef USE_OCV_KCF
-        if (!m_tracker || m_tracker.empty())
-        {
-#if (((CV_VERSION_MAJOR == 3) && (CV_VERSION_MINOR > 3)) || (CV_VERSION_MAJOR > 3))
-            m_tracker = cv::TrackerMOSSE::create();
-#else
-            m_tracker = cv::TrackerMOSSE::createTracker();
-#endif
-        }
-#endif
-        if (m_VOTTracker)
-            m_VOTTracker = nullptr;
-        break;
+    m_predictionRect3D = m_kalman.Update(region.m_rect, dataCorrect);
 
-    case tracking::TrackCSRT:
-#ifdef USE_OCV_KCF
-        if (!m_tracker || m_tracker.empty())
-        {
-#if (CV_VERSION_MAJOR >= 4)
-            cv::TrackerCSRT::Params params;
-            params.psr_threshold = 0.04f; // 0.035f;
-            if (channels == 1)
-            {
-                params.use_gray = true;
-                params.use_rgb = false;
-            }
-            else
-            {
-                params.use_gray = false;
-                params.use_rgb = true;
-            }
-            m_tracker = cv::TrackerCSRT::create(params);
-#endif
-        }
-#endif
-        if (m_VOTTracker)
-            m_VOTTracker = nullptr;
-        break;
+    m_predictionPoint = m_predictionRect3D.center;
 
-        //    case tracking::TrackDAT:
-        //#ifdef USE_OCV_KCF
-        //        if (m_tracker && !m_tracker.empty())
-        //            m_tracker.release();
-        //#endif
-        //        if (!m_VOTTracker)
-        //            m_VOTTracker = std::unique_ptr<DAT_TRACKER>(new DAT_TRACKER());
-        //        break;
-
-        //    case tracking::TrackSTAPLE:
-        //#ifdef USE_OCV_KCF
-        //        if (m_tracker && !m_tracker.empty())
-        //            m_tracker.release();
-        //#endif
-        //#ifdef USE_STAPLE_TRACKER
-        //        if (!m_VOTTracker)
-        //            m_VOTTracker = std::unique_ptr<STAPLE_TRACKER>(new STAPLE_TRACKER());
-        //#else
-        //        std::cerr << "Project was compiled without STAPLE tracking!" << std::endl;
-        //#endif
-        //        break;
-        //#if 1
-        //    case tracking::TrackLDES:
-        //#ifdef USE_OCV_KCF
-        //        if (m_tracker && !m_tracker.empty())
-        //            m_tracker.release();
-        //#endif
-        //#ifdef USE_STAPLE_TRACKER
-        //        if (!m_VOTTracker)
-        //            m_VOTTracker = std::unique_ptr<LDESTracker>(new LDESTracker());
-        //#else
-        //        std::cerr << "Project was compiled without STAPLE tracking!" << std::endl;
-        //#endif
-        //        break;
-        //#endif
-    }
+    //std::cout << "brect = " << brect << ", dx = " << dx << ", dy = " << dy << ", outOfTheFrame = " << m_outOfTheFrame << ", predictionPoint = " << m_predictionPoint << std::endl;
 }
 
 ///
@@ -815,7 +453,7 @@ void CTrack::CreateExternalTracker(int channels)
 /// \param dataCorrect
 ///
 void CTrack::PointUpdate(
-        const Point_t& pt,
+        const cv::Point3f& pt,
         const cv::Size& newObjSize,
         bool dataCorrect,
         const cv::Size& frameSize

src/fusion/lidar_radar_fusion_cnn/Tracker/track.h

@@ -11,7 +11,6 @@
 
 #include "defines.h"
 #include "Kalman.h"
-#include "VOTTracker.hpp"
 
 // --------------------------------------------------------------------------
 ///
@@ -31,7 +30,7 @@ struct TrajectoryPoint
     /// \brief TrajectoryPoint
     /// \param prediction
     ///
-    TrajectoryPoint(const Point_t& prediction)
+    TrajectoryPoint(const cv::Point3f& prediction)
         :
           m_hasRaw(false),
           m_prediction(prediction)
@@ -43,7 +42,7 @@ struct TrajectoryPoint
     /// \param prediction
     /// \param raw
     ///
-    TrajectoryPoint(const Point_t& prediction, const Point_t& raw)
+    TrajectoryPoint(const cv::Point3f& prediction, const cv::Point3f& raw)
         :
           m_hasRaw(true),
           m_prediction(prediction),
@@ -52,8 +51,8 @@ struct TrajectoryPoint
     }
 
     bool m_hasRaw = false;
-    Point_t m_prediction;
-    Point_t m_raw;
+    cv::Point3f m_prediction;
+    cv::Point3f m_raw;
 };
 
 // --------------------------------------------------------------------------
@@ -68,7 +67,7 @@ public:
     /// \param i
     /// \return
     ///
-    const Point_t& operator[](size_t i) const
+    const cv::Point3f& operator[](size_t i) const
     {
         return m_trace[i].m_prediction;
     }
@@ -78,7 +77,7 @@ public:
     /// \param i
     /// \return
     ///
-    Point_t& operator[](size_t i)
+    cv::Point3f& operator[](size_t i)
     {
         return m_trace[i].m_prediction;
     }
@@ -106,11 +105,11 @@ public:
     /// \brief push_back
     /// \param prediction
     ///
-    void push_back(const Point_t& prediction)
+    void push_back(const cv::Point3f& prediction)
     {
         m_trace.emplace_back(prediction);
     }
-    void push_back(const Point_t& prediction, const Point_t& raw)
+    void push_back(const cv::Point3f& prediction, const cv::Point3f& raw)
     {
         m_trace.emplace_back(prediction, raw);
     }
@@ -159,21 +158,20 @@ private:
 ///
 struct TrackingObject
 {
-    cv::RotatedRect m_rrect;           // Coordinates
+    Rect3D m_rect;           // Coordinates
+    CRegion m_region;         // detect region
     Trace m_trace;                     // Trajectory
     size_t m_ID = 0;                   // Objects ID
-    size_t m_regionID = 0;             // region id <M
+    size_t m_regionID = 0;                   // Objects ID
     bool m_isStatic = false;           // Object is abandoned
     bool m_outOfTheFrame = false;      // Is object out of freme
-    std::string m_type;                // Objects type name or empty value
-    float m_confidence = -1;           // From Detector with score (YOLO or SSD)
     cv::Vec<track_t, 2> m_velocity;    // pixels/sec
-
+    size_t m_detectedFrames = 0;       // detected frames' count
     ///
-    TrackingObject(const cv::RotatedRect& rrect, size_t ID, size_t regionID, const Trace& trace,
-                   bool isStatic, bool outOfTheFrame, const std::string& type, float confidence, cv::Vec<track_t, 2> velocity)
+    TrackingObject(const Rect3D& rect, const CRegion& region, size_t ID, size_t regionID, const Trace& trace,
+                   bool isStatic, bool outOfTheFrame, cv::Vec<track_t, 2> velocity, size_t detectedFrames)
         :
-          m_rrect(rrect), m_ID(ID), m_regionID(regionID), m_isStatic(isStatic), m_outOfTheFrame(outOfTheFrame), m_type(type), m_confidence(confidence), m_velocity(velocity)
+          m_rect(rect), m_region(region), m_ID(ID), m_regionID(regionID), m_isStatic(isStatic), m_outOfTheFrame(outOfTheFrame), m_velocity(velocity), m_detectedFrames(detectedFrames)
     {
         for (size_t i = 0; i < trace.size(); ++i)
         {
@@ -184,7 +182,6 @@ struct TrackingObject
                 m_trace.push_back(tp.m_prediction);
         }
     }
-
     ///
     bool IsRobust(int minTraceSize, float minRawRatio, cv::Size2f sizeRatio) const
     {
@@ -192,7 +189,7 @@ struct TrackingObject
         res &= m_trace.GetRawCount(m_trace.size() - 1) / static_cast<float>(m_trace.size()) > minRawRatio;
         if (sizeRatio.width + sizeRatio.height > 0)
         {
-            float sr = m_rrect.size.width / m_rrect.size.height;
+            float sr = m_rect.size.width / m_rect.size.height;
             if (sizeRatio.width > 0)
                 res &= (sr > sizeRatio.width);
 
@@ -220,7 +217,7 @@ public:
            bool useAcceleration,
            size_t trackID,
            size_t regionID,
-           bool filterObjectSize,
+           tracking::FilterGoal filterObjectGoal,
            tracking::LostTrackType externalTrackerForLost);
 
     ///
@@ -238,22 +235,14 @@ public:
     ///
     track_t CalcDistRect(const CRegion& reg) const;
     ///
-    /// \brief CalcDistJaccard
-    /// Jaccard distance from 0 to 1 between object bounding rectangles on two N and N+1 frames
-    /// \param reg
-    /// \return
-    ///
-    track_t CalcDistJaccard(const CRegion& reg) const;
-    ///
-    /// \brief CalcDistJaccard
-    /// Distance from 0 to 1 between objects histogramms on two N and N+1 frames
+    /// \brief CalcDist
+    /// Euclidean distance in pixels between object contours on two N and N+1 frames
     /// \param reg
-    /// \param currFrame
     /// \return
     ///
-    track_t CalcDistHist(const CRegion& reg, cv::UMat currFrame) const;
+    track_t CalcDistRect3D(const CRegion& reg) const;
 
-    cv::RotatedRect CalcPredictionEllipse(cv::Size_<track_t> minRadius) const;
+    //cv::RotatedRect CalcPredictionEllipse(cv::Size_<track_t> minRadius) const;
     ///
     /// \brief IsInsideArea
     /// Test point inside in prediction area: prediction area + object velocity
@@ -271,13 +260,14 @@ public:
     bool IsStaticTimeout(int framesTime) const;
     bool IsOutOfTheFrame() const;
 
-    cv::RotatedRect GetLastRect() const;
+    Rect3D GetLastRect() const;
 
-    const Point_t& AveragePoint() const;
-    Point_t& AveragePoint();
+    const cv::Point3f& AveragePoint() const;
+    cv::Point3f& AveragePoint();
     const CRegion& LastRegion() const;
     size_t SkippedFrames() const;
     size_t& SkippedFrames();
+    size_t& DetectedFrames();
 
     TrackingObject ConstructObject() const;
 public:
@@ -286,31 +276,28 @@ private:
     Trace m_trace;
     size_t m_trackID = 0;
     size_t m_skippedFrames = 0;
+    size_t m_detectedFrames = 0;
     CRegion m_lastRegion;
 
-    Point_t m_predictionPoint;
+    cv::Point3f m_predictionPoint;
     cv::RotatedRect m_predictionRect;
+    Rect3D m_predictionRect3D;
     TKalmanFilter m_kalman;
-    bool m_filterObjectSize = false;
+    tracking::FilterGoal m_filterGoal = tracking::FilterCenter;
     bool m_outOfTheFrame = false;
 
     tracking::LostTrackType m_externalTrackerForLost;
-#ifdef USE_OCV_KCF
-    cv::Ptr<cv::Tracker> m_tracker;
-#endif
-    std::unique_ptr<VOTTracker> m_VOTTracker;
 
     void RectUpdate(const CRegion& region, bool dataCorrect, cv::UMat prevFrame, cv::UMat currFrame);
-
-    void CreateExternalTracker(int channels);
-
-    void PointUpdate(const Point_t& pt, const cv::Size& newObjSize, bool dataCorrect, const cv::Size& frameSize);
+    void Rect3DUpdate(const CRegion& region, bool dataCorrect, cv::UMat prevFrame, cv::UMat currFrame);
+    void PointUpdate(const cv::Point3f& pt, const cv::Size& newObjSize, bool dataCorrect, const cv::Size& frameSize);
 
     bool CheckStatic(int trajLen, cv::UMat currFrame, const CRegion& region);
     bool m_isStatic = false;
     int m_staticFrames = 0;
     cv::UMat m_staticFrame;
-    cv::Rect m_staticRect;
+    Rect3D m_staticRect;
 };
 
 typedef std::vector<std::unique_ptr<CTrack>> tracks_t;
+

src/fusion/lidar_radar_fusion_cnn/fusion.hpp

@@ -49,18 +49,18 @@ void Get_AssociationMat(iv::lidar::objectarray& lidar_object_arry,iv::radar::rad
             cv::Point2f po1;
             po1.x = lidar_object_arry.obj(i).centroid().x();
             po1.y = lidar_object_arry.obj(i).centroid().y();
-            for(int index =0; index< dis.size(); index++)
+            for(int index =0; index< fuindex.size(); index++)
             {
                 cv::Point2f po2;
-                po2.x = radar_object_array.obj(dis[index]).x();
-                po2.y = radar_object_array.obj(dis[index]).y();
+                po2.x = radar_object_array.obj(fuindex[index]).x();
+                po2.y = radar_object_array.obj(fuindex[index]).y();
                 dis.push_back(ComputeDis(po1,po2));
             }
 //            std::cout<<"  x    y     :   "<<po1.x<<"   "<<po1.y<<std::endl;
             auto smallest = std::min_element(std::begin(dis), std::end(dis));
             int index_ = std::distance(std::begin(dis), smallest);
             dis.clear();
-            match.nradar = index_;
+            match.nradar = fuindex[index_];
         }else {
             match.nradar = -1000;
         }
@@ -99,27 +99,38 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
     {
         iv::fusion::fusionobject fusion_object;
         fusion_object.set_id(lidar_object_arr.obj(match_idx[i].nlidar).id());
-        fusion_object.set_type_name(lidar_object_arr.obj(match_idx[i].nlidar).type_name());
+//        fusion_object.set_sensor_type(0);
+        fusion_object.set_type(lidar_object_arr.obj(match_idx[i].nlidar).mntype());
         fusion_object.set_prob(lidar_object_arr.obj(match_idx[i].nlidar).score());
+
         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());
+            if( lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x() *
+                   lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y() > 18)
+            {
+                fusion_object.set_yaw(1.57);
+//                fusion_object.set_yaw(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
+            } else {
+                fusion_object.set_yaw(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
+            }
 
             iv::fusion::VelXY vel_relative;
             iv::fusion::VelXY *vel_relative_;
-            vel_relative.set_x(0);
-            vel_relative.set_y(-2);
+            vel_relative.set_x(lidar_object_arr.obj(match_idx[i].nlidar).vel_relative().x());
+            vel_relative.set_y(lidar_object_arr.obj(match_idx[i].nlidar).vel_relative().y());
             vel_relative_ = fusion_object.mutable_vel_relative();
             vel_relative_->CopyFrom(vel_relative);
 
             iv::fusion::PointXYZ centroid;
             iv::fusion::PointXYZ *centroid_;
-            centroid.set_x(lidar_object_arr.obj(match_idx[i].nlidar).position().x());
-            centroid.set_y(lidar_object_arr.obj(match_idx[i].nlidar).position().y());
-            centroid.set_z(lidar_object_arr.obj(match_idx[i].nlidar).position().z());
+            centroid.set_x(lidar_object_arr.obj(match_idx[i].nlidar).centroid().x());
+            centroid.set_y(lidar_object_arr.obj(match_idx[i].nlidar).centroid().y());
+            centroid.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z());
             centroid_ = fusion_object.mutable_centroid();
             centroid_->CopyFrom(centroid);
+            std::cout<<"lidar: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
+
         }else {
 //             std::cout<<"   fusion    is    ok  "<<std::endl;
             fusion_object.set_yaw(radar_object_array.obj(match_idx.at(i).nradar).angle());
@@ -129,8 +140,8 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
             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.set_x(0);
-            vel_relative.set_y(2);
+            vel_relative.set_x(lidar_object_arr.obj(match_idx[i].nlidar).vel_relative().x());
+            vel_relative.set_y(lidar_object_arr.obj(match_idx[i].nlidar).vel_relative().y());
             vel_relative_ = fusion_object.mutable_vel_relative();
             vel_relative_->CopyFrom(vel_relative);
 
@@ -148,6 +159,9 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
             centroid.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z());
             centroid_ = fusion_object.mutable_centroid();
             centroid_->CopyFrom(centroid);
+#ifdef DEBUG_SHOW
+            std::cout<<"radar: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
+#endif
         }
 
         iv::fusion::Dimension dimension;
@@ -158,60 +172,10 @@ 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).dimensions().x() *
-               lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y() < 0.25)
-        {
-            fusion_object.set_type(2);
-        } else {
-            fusion_object.set_type(1);
-        }
+        std::cout<<" x     y    z:   "<<lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()<<"     "<<lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()<<std::endl;
 
 
 
-        if(lidar_object_arr.obj(match_idx[i].nlidar).position().y() > 20 || abs(lidar_object_arr.obj(match_idx[i].nlidar).position().x())>10 ) continue;
-        if (lidar_object_arr.obj(match_idx[i].nlidar).position().y()>10 && abs(lidar_object_arr.obj(match_idx[i].nlidar).position().x())<1.3) continue;
-        if(lidar_object_arr.obj(match_idx[i].nlidar).position().y() <1.0  && abs(lidar_object_arr.obj(match_idx[i].nlidar).position().x())<1.3) continue;
-        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;
-            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;
-           float yaw;
-           if( lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x() *
-                  lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y() > 8)
-           {
-               yaw = 1.57;
-           } else {
-               yaw = lidar_object_arr.obj(match_idx[i].nlidar).tyaw();
-           }
-            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(yaw) -
-                            nomal_y*sin(yaw);
-                    float t = nomal_x*sin(yaw) +
-                            nomal_y*cos(yaw);
-                    if(abs(lidar_object_arr.obj(match_idx[i].nlidar).centroid().x() + s) <1.3 &&
-                            lidar_object_arr.obj(match_idx[i].nlidar).centroid().y() + t <1.0) continue;
-                    else{
-                        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);
     }
@@ -242,31 +206,6 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
         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);
     }
@@ -280,7 +219,8 @@ void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array,
 //    lidar_radar_fusion_object_array.Clear();
     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_sensor_type(1);
+        fusion_obj.set_yaw(0);
         iv::fusion::VelXY vel_relative;
         iv::fusion::VelXY *vel_relative_;
         if(xobs_info.xobj(j).obstype() == iv::mobileye::obs_OBSTYPE::obs_OBSTYPE_PED)
@@ -301,6 +241,9 @@ void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array,
         centroid.set_z(1.0);
         centroid_ = fusion_obj.mutable_centroid();
         centroid_->CopyFrom(centroid);
+#ifdef DEBUG_SHOW
+        std::cout<<"mobileye: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
+#endif
 
         iv::fusion::Dimension dimension;
         iv::fusion::Dimension *dimension_;
@@ -309,37 +252,58 @@ void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array,
         dimension.set_z(1.0);
         dimension_ = fusion_obj.mutable_dimensions();
         dimension_->CopyFrom(dimension);
+        iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
+        obj->CopyFrom(fusion_obj);
+    }
+
+}
 
-        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++)
+void ObsToNormal(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
+{
+    for(int i = 0; i < lidar_radar_fusion_object_array.obj_size(); i++)
+    {
+//        if((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0) &&(lidar_radar_fusion_object_array.obj(i).centroid().y() > 20 || abs(lidar_radar_fusion_object_array.obj(i).centroid().x())>10) ) continue;
+//        if ((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0)&&(lidar_radar_fusion_object_array.obj(i).centroid().y()>10 && abs(lidar_radar_fusion_object_array.obj(i).centroid().x())<1.3)) continue;
+//        if((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0)&&(lidar_radar_fusion_object_array.obj(i).centroid().y() <1.0  && abs(lidar_radar_fusion_object_array.obj(i).centroid().x())<1.3)) continue;
+
+        if((lidar_radar_fusion_object_array.obj(i).dimensions().x()>0)&&
+                (lidar_radar_fusion_object_array.obj(i).dimensions().y()>0))
         {
-            for(iy = 0; iy<(yp*2); iy++)
+            int xp = (int)((lidar_radar_fusion_object_array.obj(i).dimensions().x()/0.2)/2.0);
+            if(xp == 0)xp=1;
+            int yp = (int)((lidar_radar_fusion_object_array.obj(i).dimensions().y()/0.2)/2.0);
+            if(yp == 0)yp=1;
+            int ix,iy;
+            for(ix = 0; ix<(xp*2); ix++)
             {
-                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);
+                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(lidar_radar_fusion_object_array.obj(i).yaw())
+                            - nomal_y*sin(lidar_radar_fusion_object_array.obj(i).yaw());
+                    float t = nomal_x*sin(lidar_radar_fusion_object_array.obj(i).yaw())
+                            + nomal_y*cos(lidar_radar_fusion_object_array.obj(i).yaw());
+                    nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s);
+                    nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t);
+                    if(abs(lidar_radar_fusion_object_array.obj(i).centroid().x() + s) <1.3 &&
+                            lidar_radar_fusion_object_array.obj(i).centroid().y() + t <1.0) continue;
+                    else{
+                        nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s);
+                        nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t);
+                        iv::fusion::fusionobject &fusion_obj = (iv::fusion::fusionobject &)lidar_radar_fusion_object_array.obj(i);
+                        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/fusion_probabilities.cpp

@@ -9,7 +9,7 @@ CONFIG -= app_bundle
 # depend on your compiler). Please consult the documentation of the
 # deprecated API in order to know how to port your code away from it.
 DEFINES += QT_DEPRECATED_WARNINGS
-
+DEFINES += DEBUG_SHOW
 INCLUDEPATH +=Tracker
 
 HEADERS += \
@@ -28,12 +28,11 @@ HEADERS += \
     Tracker/Kalman.h \
     Tracker/ShortPathCalculator.h \
     Tracker/track.h \
-    Tracker/VOTTracker.hpp \
-    Tracker/Tracking.h \
     ../../include/msgtype/mobileye.pb.h \
     ../../include/msgtype/mobileye_lane.pb.h \
     ../../include/msgtype/mobileye_obs.pb.h \
-    ../../include/msgtype/mobileye_tsr.pb.h
+    ../../include/msgtype/mobileye_tsr.pb.h \
+    Tracker/Tracking.hpp
 
 
 SOURCES += \

src/fusion/lidar_radar_fusion_cnn/main.cpp

@@ -12,11 +12,12 @@
 #include "eigen3/Eigen/Dense"
 //#include "data_manager.h"
 #include "fusion.hpp"
-#include "Tracking.h"
+#include "Tracking.hpp"
 #include "transformation.h"
 #include "mobileye.pb.h"
 using namespace iv;
 using namespace fusion;
+ImmUkfPda giup;
 
 void *gfu = iv::modulecomm::RegisterSend("li_ra_fusion",10000000,1);
 static QMutex gMutex;
@@ -31,6 +32,7 @@ iv::mobileye::mobileye mobileye_info;
 iv::mobileye::obs obs_info;
 iv::mobileye::lane lane_info;
 iv::mobileye::tsr tsr_info;
+bool m_isTrackerInitialized = false;
 
 
 QTime gTime;
@@ -56,20 +58,8 @@ void Listenesrfront(const char * strdata,const unsigned int nSize,const unsigned
     else{
         //std::<<"srfront byte size:  "<<radarobjvec.ByteSize()<<std::endl;
     }
-//        std::cout<<"radar is ok :  "<<radarobj.obj_size()<<std::endl;
+    //        std::cout<<"radar is ok :  "<<radarobj.obj_size()<<std::endl;
     gMutex.lock();
-//    for(int i=0;i<radarobj.obj_size();i++)
-//    {
-//        if(abs(radarobj.obj(i).x())<2 && abs(radarobj.obj(i).vy()) >10)
-//        {
-//            std::cout<<"x    y      vx    vy  "<<radarobj.obj(i).x()<<"          "
-//                    <<radarobj.obj(i).y()<<"         "
-//                    <<radarobj.obj(i).vx()<<"     "
-//                    <<radarobj.obj(i).vy()<<"    "<<std::endl;
-
-//        }
-
-//    }
 
     radarobjvec.CopyFrom(radarobj);
     gMutex.unlock();
@@ -85,9 +75,9 @@ void Listenlidarcnndetect(const char * strdata,const unsigned int nSize,const un
         std::cout<<"PecerptionShow Listenesrfront fail."<<std::endl;
         return;
     }
-//    std::cout<<"  lidar  is  ok   "<<lidarobj.obj_size()<<std::endl;
+    //    std::cout<<"  lidar  is  ok   "<<lidarobj.obj_size()<<std::endl;
     gMutex.lock();
-//    std::cout<<"   obj size  "<<lidarobj.obj_size()<<std::endl;
+    //    std::cout<<"   obj size  "<<lidarobj.obj_size()<<std::endl;
     datafusion(lidarobj,radarobjvec,li_ra_fusion);
     gMutex.unlock();
 }
@@ -102,14 +92,6 @@ void Listenmobileye(const char * strdata,const unsigned int nSize,const unsigned
         std::cout<<"PecerptionShow Listenesrfront fail."<<std::endl;
         return;
     }
-//    std::cout<< " obj size   "<<mobileye.xobj_size()<<std::endl;
-//    for (int m_index = 0; m_index< mobileye.xobj_size(); m_index++)
-//    {
-//        std::cout<<" p_x  p_y  v_x  v_y   "<<mobileye.xobj(m_index).pos_y()<< "  "<<mobileye.xobj(m_index).pos_x()
-//                <<"    "<<mobileye.xobj(m_index).obs_rel_vel_x() * tan(mobileye.xobj(m_index).obsang())
-//               <<"   "<<mobileye.xobj(m_index).obs_rel_vel_x()<<"                                   \n"
-//               << mobileye.xobj(m_index).obsang()<<std::endl;
-//    }
 
     gMutex.lock();
     mobileye_info.CopyFrom(mobileye);
@@ -119,64 +101,19 @@ void Listenmobileye(const char * strdata,const unsigned int nSize,const unsigned
 int ccccc =0;
 void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& radarobjvec, iv::fusion::fusionobjectarray& li_ra_fusion)
 {
-//    gMutex.lock();
-//    Transformation::RadarPross(radarobjvec);
-
-    RLfusion(lidar_obj,radarobjvec,li_ra_fusion);
-
-
-//    li_ra_fusion.Clear();
-//      std::cout<<" RLFusion     begin    "<<std::endl;
-    AddMobileye(li_ra_fusion,mobileye_info);
-//    std::cout<<" RLFusion     end      "<<std::endl;
-    mobileye_info.clear_xobj();
-//        std::cout<< "     fusion size  "<<li_ra_fusion.obj_size()<<std::endl;
-//    for(int i=0;i<li_ra_fusion.obj_size();i++)
-//    {
-//        if(abs(li_ra_fusion.obj(i).vel_relative().y())>10 )
-//        {
-//            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;
-//    for(int nradar = 0; nradar<radarobjvec.obj_size(); nradar++)
-//    {
-//        if(radarobjvec.obj(nradar).bvalid() == false) continue;
-//        if(sqrt(abs(radarobjvec.obj(nradar).x()))< 4 && radarobjvec.obj(nradar).y()< 20 ) nsize = nsize +1;
-//    }
-
-//        std::cout<<"   fusion_obj_size     radar_obj_size   :   "<<li_ra_fusion.obj_size()<<"     "<<nsize<<std::endl;
 
+    RLfusion(lidar_obj,radarobjvec,li_ra_fusion);
+    giup.call(li_ra_fusion);
 
-//    gMutex.lock();
-//    std::cout<<" track   begin"<<std::endl;
-    //---------------------------------------------  init tracker  -------------------------------------------------
-    if (!m_isTrackerInitialized)
-    {
-        m_isTrackerInitialized = InitTracker(tracker);
-        if (!m_isTrackerInitialized)
-        {
-            std::cerr << "Tracker initialize error!!!" << std::endl;
-        }
-    }
-    Tracking(li_ra_fusion, tracker);
-//    std::<<"track    end"<<std::endl;
 
-    //--------------------------------------------  end tracking  --------------------------------------------------
-//    gMutex.unlock();
+    iv::fusion::fusionobjectarray out_fusion = trackedobjvec;
+    ObsToNormal(out_fusion);
     string out;
 
-    if(li_ra_fusion.obj_size() == 0)
+    if(out_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_;
@@ -188,20 +125,15 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
         fake_cen_ = fake_obj.mutable_centroid();
         fake_cen_ ->CopyFrom(fake_cen);
 
-        fake_obj_ = li_ra_fusion.add_obj();
+        fake_obj_ = out_fusion.add_obj();
         fake_obj_->CopyFrom(fake_obj);
-        out = li_ra_fusion.SerializeAsString();
+        out = out_fusion.SerializeAsString();
     }else
     {
-        out = li_ra_fusion.SerializeAsString();
-//        for (int k = 0; k<li_ra_fusion.obj_size();k++){
-//            std::cout<<" size   x    y    vy :   "<<li_ra_fusion.obj_size()<<"   "
-//                    <<li_ra_fusion.obj(k).centroid().x()<< "   "<<li_ra_fusion.obj(k).centroid().y()<<"    "
-//                   <<li_ra_fusion.obj(k).vel_relative().y()<<"   "<<li_ra_fusion.obj(k).nomal_centroid_size() <<std::endl;
-//        }
+        out = out_fusion.SerializeAsString();
     }
     iv::modulecomm::ModuleSendMsg(gfu,out.data(),out.length());
-//    gMutex.unlock();
+    //    gMutex.unlock();
 }
 
 

src/fusion/lidar_radar_fusion_cnn/transformation.cpp

@@ -7,7 +7,7 @@
 #include "objectarray.pb.h"
 #include "mobileye.pb.h"
 
-#include "Eigen/Eigen"
+//#include "Eigen/Eigen"
 
 namespace iv {
 namespace fusion {
@@ -25,7 +25,7 @@ float ComputeDis(cv::Point2f po1, cv::Point2f po2)
 void Get_AssociationMat(iv::lidar::objectarray& lidar_object_arry,iv::radar::radarobjectarray& radar_object_array,
                         std::vector<match_index>& match_idx,std::vector<int>&radar_idx)
 {
-    std::cout<<" get mat  begin  "<<std::endl;
+//    std::cout<<" get mat  begin  "<<std::endl;
     int nlidar = lidar_object_arry.obj_size();
     int nradar = radar_object_array.obj_size();
     match_idx.clear();
@@ -49,18 +49,18 @@ void Get_AssociationMat(iv::lidar::objectarray& lidar_object_arry,iv::radar::rad
             cv::Point2f po1;
             po1.x = lidar_object_arry.obj(i).centroid().x();
             po1.y = lidar_object_arry.obj(i).centroid().y();
-            for(int index =0; index< dis.size(); index++)
+            for(int index =0; index< fuindex.size(); index++)
             {
                 cv::Point2f po2;
-                po2.x = radar_object_array.obj(dis[index]).x();
-                po2.y = radar_object_array.obj(dis[index]).y();
+                po2.x = radar_object_array.obj(fuindex[index]).x();
+                po2.y = radar_object_array.obj(fuindex[index]).y();
                 dis.push_back(ComputeDis(po1,po2));
             }
 //            std::cout<<"  x    y     :   "<<po1.x<<"   "<<po1.y<<std::endl;
             auto smallest = std::min_element(std::begin(dis), std::end(dis));
             int index_ = std::distance(std::begin(dis), smallest);
             dis.clear();
-            match.nradar = index_;
+            match.nradar = fuindex[index_];
         }else {
             match.nradar = -1000;
         }
@@ -85,12 +85,12 @@ void Get_AssociationMat(iv::lidar::objectarray& lidar_object_arry,iv::radar::rad
             }
         }}
 //            std::cout<<"   radar_size   :   "<<"    "<<radar_idx.size()<<std::endl;
-    std::cout<<"   get  mat end  "<<std::endl;
+//    std::cout<<"   get  mat end  "<<std::endl;
 }
 
 void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarray& radar_object_array, iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
 {
-    std::cout<< "    get   fusion  begin   "<<std::endl;
+//    std::cout<< "    get   fusion  begin   "<<std::endl;
     lidar_radar_fusion_object_array.Clear();
     std::vector<match_index> match_idx;
     std::vector<int> radar_idx;
@@ -99,16 +99,33 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
     {
         iv::fusion::fusionobject fusion_object;
         fusion_object.set_id(lidar_object_arr.obj(match_idx[i].nlidar).id());
-        fusion_object.set_type_name(lidar_object_arr.obj(match_idx[i].nlidar).type_name());
+//        fusion_object.set_sensor_type(0);
+        fusion_object.set_type(lidar_object_arr.obj(match_idx[i].nlidar).mntype());
         fusion_object.set_prob(lidar_object_arr.obj(match_idx[i].nlidar).score());
+
         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).angle());
+            if( lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x() *
+                   lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y() > 18)
+            {
+
+//                fusion_object.set_yaw(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
+//                if(abs(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()) >=0.785)
+                    fusion_object.set_yaw(1.57);
+//                else if (abs(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()) <0.785){
+//                    fusion_object.set_yaw(0.3);
+//                }
+
+
+            } else {
+                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).tyaw());
             iv::fusion::VelXY vel_relative;
             iv::fusion::VelXY *vel_relative_;
-            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.set_x(lidar_object_arr.obj(match_idx[i].nlidar).vel_relative().x());
+            vel_relative.set_y(lidar_object_arr.obj(match_idx[i].nlidar).vel_relative().y());
             vel_relative_ = fusion_object.mutable_vel_relative();
             vel_relative_->CopyFrom(vel_relative);
 
@@ -119,6 +136,8 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
             centroid.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z());
             centroid_ = fusion_object.mutable_centroid();
             centroid_->CopyFrom(centroid);
+//            std::cout<<"lidar: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
+
         }else {
 //             std::cout<<"   fusion    is    ok  "<<std::endl;
             fusion_object.set_yaw(radar_object_array.obj(match_idx.at(i).nradar).angle());
@@ -126,8 +145,10 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
 
             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.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.set_x(lidar_object_arr.obj(match_idx[i].nlidar).vel_relative().x());
+            vel_relative.set_y(lidar_object_arr.obj(match_idx[i].nlidar).vel_relative().y());
             vel_relative_ = fusion_object.mutable_vel_relative();
             vel_relative_->CopyFrom(vel_relative);
 
@@ -145,81 +166,28 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
             centroid.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z());
             centroid_ = fusion_object.mutable_centroid();
             centroid_->CopyFrom(centroid);
+#ifdef DEBUG_SHOW
+//            std::cout<<"radar: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
+#endif
         }
 
-        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_;
-//        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()<<"    "<<
-               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();
-        }
+//        std::cout<<" x     y    z:   "<<lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()<<"     "<<lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()<<std::endl;
 
 
-        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);
     }
     for(int j = 0; j< radar_idx.size() ; j++){
+        break;
         iv::fusion::fusionobject fusion_obj;
         fusion_obj.set_yaw(radar_object_array.obj(radar_idx[j]).angle());
         iv::fusion::VelXY vel_relative;
@@ -245,108 +213,52 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
         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);
     }
-    std::cout<<"   fusion   end    "<<std::endl;
-}
-}
+//    std::cout<<"   fusion   end    "<<std::endl;
 }
 
+
 void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array, iv::mobileye::mobileye& xobs_info)
 {
     int time_step = 0.3;
+//    lidar_radar_fusion_object_array.Clear();
     for(int j = 0; j< xobs_info.xobj_size() ; j++){
         iv::fusion::fusionobject fusion_obj;
+//        fusion_obj.set_sensor_type(1);
         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()));
-//        vel_relative.set_x(xobs_info.xobj(j).obs_rel_vel_x());
-        vel_relative.set_y(xobs_info.xobj(j).obs_rel_vel_x());
+        if(xobs_info.xobj(j).obstype() == iv::mobileye::obs_OBSTYPE::obs_OBSTYPE_PED)
+        {
+            fusion_obj.set_type(2);
+        }else{
+            fusion_obj.set_type(1);
+            vel_relative.set_x(xobs_info.xobj(j).obs_rel_vel_x() * tan(xobs_info.xobj(j).obsang()));
+            vel_relative.set_y(xobs_info.xobj(j).obs_rel_vel_x());
+        }
         vel_relative_ = fusion_obj.mutable_vel_relative();
         vel_relative_->CopyFrom(vel_relative);
 
         iv::fusion::PointXYZ centroid;
         iv::fusion::PointXYZ *centroid_;
-        centroid.set_x(-xobs_info.xobj(j).pos_y());
+        centroid.set_x(-(xobs_info.xobj(j).pos_y()));
         centroid.set_y(xobs_info.xobj(j).pos_x());
         centroid.set_z(1.0);
         centroid_ = fusion_obj.mutable_centroid();
         centroid_->CopyFrom(centroid);
+#ifdef DEBUG_SHOW
+        std::cout<<"mobileye: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
+#endif
 
         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);
-//        }
         dimension.set_x(xobs_info.xobj(j).obswidth());
-        dimension.set_y(2.5);
-
+        dimension.set_y(2.0);
         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);
     }
@@ -354,4 +266,8 @@ void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array,
 }
 
 
+
+}
+}
+
 #endif // FUSION_HPP

src/fusion/lidar_radar_fusion_cnn_ukf/imageBuffer.h

@@ -1,84 +0,0 @@
-#ifndef IMAGEBUFFER_H
-#define IMAGEBUFFER_H
-
-#include <opencv2/opencv.hpp>
-#include <mutex>
-#include <condition_variable>
-#include <queue>
-
-
-template<typename T>
-class ConsumerProducerQueue
-{
-
-public:
-    ConsumerProducerQueue(int mxsz,bool dropFrame) :
-            maxSize(mxsz),dropFrame(dropFrame)
-    { }
-
-    bool add(T request)
-    {
-        std::unique_lock<std::mutex> lock(mutex);
-        if(dropFrame && isFull())
-        {
-            lock.unlock();
-			return false;
-        }
-        else {
-            cond.wait(lock, [this]() { return !isFull(); });
-            cpq.push(request);
-            //lock.unlock();
-            cond.notify_all();
-            return true;
-        }
-    }
-
-    void consume(T &request)
-    {
-        std::unique_lock<std::mutex> lock(mutex);
-        cond.wait(lock, [this]()
-        { return !isEmpty(); });
-        request = cpq.front();
-        cpq.pop();
-        //lock.unlock();
-        cond.notify_all();
-
-    }
-
-    bool isFull() const
-    {
-        return cpq.size() >= maxSize;
-    }
-
-    bool isEmpty() const
-    {
-        return cpq.size() == 0;
-    }
-
-    int length() const
-    {
-        return cpq.size();
-    }
-
-    void clear()
-    {
-        std::unique_lock<std::mutex> lock(mutex);
-        while (!isEmpty())
-        {
-            cpq.pop();
-        }
-        lock.unlock();
-        cond.notify_all();
-    }
-
-private:
-    std::condition_variable cond;
-    std::mutex mutex;
-    std::queue<T> cpq;
-    int maxSize;
-    bool dropFrame;
-};
-
-
-
-#endif

src/fusion/lidar_radar_fusion_cnn_ukf/imm_ukf_pda.cpp

@@ -82,6 +82,54 @@ void ImmUkfPda::run()
 //  }
 }
 
+
+void ObsToNormal(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
+{
+    for(int i = 0; i < lidar_radar_fusion_object_array.obj_size(); i++)
+    {
+//        if((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0) &&(lidar_radar_fusion_object_array.obj(i).centroid().y() > 20 || abs(lidar_radar_fusion_object_array.obj(i).centroid().x())>10) ) continue;
+//        if ((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0)&&(lidar_radar_fusion_object_array.obj(i).centroid().y()>10 && abs(lidar_radar_fusion_object_array.obj(i).centroid().x())<1.3)) continue;
+//        if((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0)&&(lidar_radar_fusion_object_array.obj(i).centroid().y() <1.0  && abs(lidar_radar_fusion_object_array.obj(i).centroid().x())<1.3)) continue;
+
+        if((lidar_radar_fusion_object_array.obj(i).dimensions().x()>0)&&
+                (lidar_radar_fusion_object_array.obj(i).dimensions().y()>0))
+        {
+            int xp = (int)((lidar_radar_fusion_object_array.obj(i).dimensions().x()/0.2)/2.0);
+            if(xp == 0)xp=1;
+            int yp = (int)((lidar_radar_fusion_object_array.obj(i).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 = 1.0;
+                    float s = nomal_x*cos(lidar_radar_fusion_object_array.obj(i).yaw())
+                            - nomal_y*sin(lidar_radar_fusion_object_array.obj(i).yaw());
+                    float t = nomal_x*sin(lidar_radar_fusion_object_array.obj(i).yaw())
+                            + nomal_y*cos(lidar_radar_fusion_object_array.obj(i).yaw());
+                    nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s);
+                    nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t);
+                    if(abs(lidar_radar_fusion_object_array.obj(i).centroid().x() + s) <1.3 &&
+                            lidar_radar_fusion_object_array.obj(i).centroid().y() + t <1.0) continue;
+                    else{
+                        nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s);
+                        nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t);
+                        iv::fusion::fusionobject &fusion_obj = (iv::fusion::fusionobject &)lidar_radar_fusion_object_array.obj(i);
+                        nomal_centroid_ = fusion_obj.add_nomal_centroid();
+                        nomal_centroid_->CopyFrom(nomal_centroid);
+                    }
+                }
+            }
+
+    }
+}}
+
+
 void ImmUkfPda::callback(const iv::fusion::fusionobjectarray & input)
 {
 //  input_header_ = input.header;
@@ -941,12 +989,12 @@ 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;
-  }
-
+//  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;
+//  }
 
+//  ObsToNormal(fused_objects_output);
   std::string out = fused_objects_output.SerializeAsString();
 //   char * strout = lidarobjtostr(lidarobjvec,ntlen);
   iv::modulecomm::ModuleSendMsg(gpatrack,out.data(),out.length());

src/fusion/lidar_radar_fusion_cnn_ukf/imm_ukf_pda.h

@@ -1,66 +0,0 @@
-QT -= gui
-
-CONFIG += c++14 console
-CONFIG -= app_bundle
-
-# The following define makes your compiler emit warnings if you use
-# any feature of Qt which as been marked deprecated (the exact warnings
-# depend on your compiler). Please consult the documentation of the
-# deprecated API in order to know how to port your code away from it.
-DEFINES += QT_DEPRECATED_WARNINGS
-
-INCLUDEPATH +=Tracker
-
-HEADERS += \
-    ../../include/msgtype/fusionobject.pb.h \
-    ../../include/msgtype/fusionobjectarray.pb.h \
-    ../../include/msgtype/mobileye.pb.h \
-    ../../include/msgtype/mobileye_lane.pb.h \
-    ../../include/msgtype/mobileye_obs.pb.h \
-    ../../include/msgtype/mobileye_tsr.pb.h \
-    ../../include/msgtype/object.pb.h \
-    ../../include/msgtype/objectarray.pb.h \
-    ../../include/msgtype/radarobject.pb.h \
-    ../../include/msgtype/radarobjectarray.pb.h \
-    fusion.hpp \
-    fusion_probabilities.h \
-    imm_ukf_pda.h \
-    transformation.h \
-    ukf.h
-
-
-SOURCES += \
-        ../../include/msgtype/fusionobject.pb.cc \
-        ../../include/msgtype/fusionobjectarray.pb.cc \
-    ../../include/msgtype/mobileye.pb.cc \
-    ../../include/msgtype/mobileye_lane.pb.cc \
-    ../../include/msgtype/mobileye_obs.pb.cc \
-    ../../include/msgtype/mobileye_tsr.pb.cc \
-    ../../include/msgtype/object.pb.cc \
-    ../../include/msgtype/objectarray.pb.cc \
-        ../../include/msgtype/radarobject.pb.cc \
-        ../../include/msgtype/radarobjectarray.pb.cc \
-        fusion_probabilities.cpp \
-    imm_ukf_pda.cpp \
-        main.cpp \
-        transformation.cpp \
-    ukf.cpp
-
-!include(../../../include/ivboost.pri ) {
-    error( "Couldn't find the ivboost.pri file!" )
-}
-
-INCLUDEPATH += /usr/include/eigen3
-INCLUDEPATH += /usr/include/pcl-1.8
-INCLUDEPATH += $$PWD/../../include/msgtype
-
-LIBS += -L/usr/local/lib -lprotobuf
-
-INCLUDEPATH += $$PWD/../../../include/
-
-LIBS += -L$$PWD/../../../bin -lxmlparam -lmodulecomm -livlog -livfault
-
-
-INCLUDEPATH += /usr/include/opencv4/
-LIBS += /usr/lib/aarch64-linux-gnu/libopencv*.so
-

src/fusion/lidar_radar_fusion_cnn_ukf/main.cpp

@@ -12,15 +12,13 @@
 #include "eigen3/Eigen/Dense"
 //#include "data_manager.h"
 #include "fusion.hpp"
+#include "imm_ukf_pda.h"
 #include "transformation.h"
 #include "mobileye.pb.h"
-#include "imm_ukf_pda.h"
 using namespace iv;
 using namespace fusion;
 
 
-ImmUkfPda giup;
-
 void *gpatrack = iv::modulecomm::RegisterSend("li_ra_fusion",10000000,1);
 static QMutex gMutex;
 
@@ -34,16 +32,62 @@ iv::mobileye::mobileye mobileye_info;
 iv::mobileye::obs obs_info;
 iv::mobileye::lane lane_info;
 iv::mobileye::tsr tsr_info;
-
+bool m_isTrackerInitialized = false;
+ImmUkfPda giup;
 
 QTime gTime;
 using namespace std;
 int gntemp = 0;
 iv::fusion::fusionobjectarray li_ra_fusion;
 void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& radarobjvec, iv::fusion::fusionobjectarray& li_ra_fusion);
-bool m_isTrackerInitialized = false;
 
 
+void ObsToNormal_(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
+{
+    for(int i = 0; i < lidar_radar_fusion_object_array.obj_size(); i++)
+    {
+//        if((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0) &&(lidar_radar_fusion_object_array.obj(i).centroid().y() > 20 || abs(lidar_radar_fusion_object_array.obj(i).centroid().x())>10) ) continue;
+//        if ((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0)&&(lidar_radar_fusion_object_array.obj(i).centroid().y()>10 && abs(lidar_radar_fusion_object_array.obj(i).centroid().x())<1.3)) continue;
+//        if((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0)&&(lidar_radar_fusion_object_array.obj(i).centroid().y() <1.0  && abs(lidar_radar_fusion_object_array.obj(i).centroid().x())<1.3)) continue;
+
+        if((lidar_radar_fusion_object_array.obj(i).dimensions().x()>0)&&
+                (lidar_radar_fusion_object_array.obj(i).dimensions().y()>0))
+        {
+            int xp = (int)((lidar_radar_fusion_object_array.obj(i).dimensions().x()/0.2)/2.0);
+            if(xp == 0)xp=1;
+            int yp = (int)((lidar_radar_fusion_object_array.obj(i).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 = 1.0;
+                    float s = nomal_x*cos(lidar_radar_fusion_object_array.obj(i).yaw())
+                            - nomal_y*sin(lidar_radar_fusion_object_array.obj(i).yaw());
+                    float t = nomal_x*sin(lidar_radar_fusion_object_array.obj(i).yaw())
+                            + nomal_y*cos(lidar_radar_fusion_object_array.obj(i).yaw());
+                    nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s);
+                    nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t);
+                    if(abs(lidar_radar_fusion_object_array.obj(i).centroid().x() + s) <1.3 &&
+                            lidar_radar_fusion_object_array.obj(i).centroid().y() + t <1.0) continue;
+                    else{
+                        nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s);
+                        nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t);
+                        iv::fusion::fusionobject &fusion_obj = (iv::fusion::fusionobject &)lidar_radar_fusion_object_array.obj(i);
+                        nomal_centroid_ = fusion_obj.add_nomal_centroid();
+                        nomal_centroid_->CopyFrom(nomal_centroid);
+                    }
+                }
+            }
+
+    }
+}}
+
 void Listenesrfront(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
 
@@ -57,8 +101,9 @@ void Listenesrfront(const char * strdata,const unsigned int nSize,const unsigned
     else{
         //std::<<"srfront byte size:  "<<radarobjvec.ByteSize()<<std::endl;
     }
-//        std::cout<<"radar is ok :  "<<radarobj.obj_size()<<std::endl;
+    //        std::cout<<"radar is ok :  "<<radarobj.obj_size()<<std::endl;
     gMutex.lock();
+
     radarobjvec.CopyFrom(radarobj);
     gMutex.unlock();
 }
@@ -66,67 +111,45 @@ void Listenesrfront(const char * strdata,const unsigned int nSize,const unsigned
 void Listenlidarcnndetect(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
     iv::lidar::objectarray lidarobj;
+
     if(nSize<1)return;
     if(false == lidarobj.ParseFromArray(strdata,nSize))
     {
         std::cout<<"PecerptionShow Listenesrfront fail."<<std::endl;
         return;
     }
-//    std::cout<<"  lidar  is  ok   "<<lidarobj.obj_size()<<std::endl;
+    //    std::cout<<"  lidar  is  ok   "<<lidarobj.obj_size()<<std::endl;
     gMutex.lock();
-    lidar_obj.CopyFrom(lidarobj);
-//    datafusion(lidarobj,radarobjvec,li_ra_fusion);
+    //    std::cout<<"   obj size  "<<lidarobj.obj_size()<<std::endl;
+    datafusion(lidarobj,radarobjvec,li_ra_fusion);
     gMutex.unlock();
 }
 
 void Listenmobileye(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
     iv::mobileye::mobileye mobileye;
+
     if(nSize<1)return;
     if(false == mobileye.ParseFromArray(strdata,nSize))
     {
         std::cout<<"PecerptionShow Listenesrfront fail."<<std::endl;
         return;
     }
+
     gMutex.lock();
     mobileye_info.CopyFrom(mobileye);
-    datafusion(lidar_obj,radarobjvec,li_ra_fusion);
     gMutex.unlock();
 }
 
 int ccccc =0;
 void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& radarobjvec, iv::fusion::fusionobjectarray& li_ra_fusion)
 {
-//    gMutex.lock();
-//    Transformation::RadarPross(radarobjvec);
-    RLfusion(lidar_obj,radarobjvec,li_ra_fusion);
-    AddMobileye(li_ra_fusion,mobileye_info);
-//    giup.call(li_ra_fusion);
 
 
-    string out;
+    RLfusion(lidar_obj,radarobjvec,li_ra_fusion);
+    ObsToNormal_(li_ra_fusion);
+    giup.call(li_ra_fusion);
 
-    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();
 }
 
 

src/fusion/lidar_radar_fusion_cnn_ukf/perceptionoutput.cpp

@@ -1,8 +0,0 @@
-#include "perceptionoutput.h"
-
-iv::Perception::PerceptionOutput::PerceptionOutput()
-{
-
-}
-
-

src/fusion/lidar_radar_fusion_cnn_ukf/ukf.cpp

@@ -17,7 +17,7 @@
 #define VIEW_WIDTH 2000
 #define VIEW_HEIGHT 2000
 //ConsumerProducerQueue<cv::Mat> *imageBuffer = new ConsumerProducerQueue<cv::Mat>(1, true);
-std::string gstrperceptitle[] = {"image00","lidar","radar","lidarobj","fusion","imgdet"};
+std::string gstrperceptitle[] = {"image00","lidar_track","radar","lidarobj","fusion","imgdet"};
 //std::string gstrperceptitle[] = {"image00","lidar","radar","fusion","lidarobj","imgdet"};
 //std::string gstrmemname[] = {"image00","lidarpc_center","radar","radafds","fusion"};
 std::string gstrmemname[] = {"image00","lidarpc_center","radar","fusion"};
@@ -265,7 +265,7 @@ MainWindow::MainWindow(QWidget *parent) :
 
 //   pa = iv::modulecomm::RegisterRecv("can0",Listenesrfront);
 
-    pa = iv::modulecomm::RegisterRecv("lidar_pointpillar",Listenlidarcnndectect);
+    pa = iv::modulecomm::RegisterRecv("lidar_track",Listenlidarcnndectect);
 
     pa = iv::modulecomm::RegisterRecv("fusion",ListenFusion);