remove useless foder

src/fusion/fusion_lidar_radar_camera/ekftools.cpp

@@ -1,82 +0,0 @@
-#include <iostream>
-#include "ekftools.h"
-
-using Eigen::VectorXd;
-using Eigen::MatrixXd;
-using namespace std;
-
-EKFTools::EKFTools() {}
-
-EKFTools::~EKFTools() {}
-
-VectorXd EKFTools::CalculateRMSE(const vector<VectorXd> &estimations,
-                              const vector<VectorXd> &ground_truth) {
-  /**
-    * Calculate the RMSE here.
-  */
-
-  VectorXd rmse(4);
-  rmse << 0,0,0,0;
-
-  // check the validity of the following inputs:
-  //  * the estimation vector size should not be zero
-  //  * the estimation vector size should equal ground truth vector size
-  if(estimations.size() != ground_truth.size() || estimations.size() == 0){
-    std::cout << "Invalid estimation or ground_truth data" << std::endl;
-    return rmse;
-  }
-
-  //accumulate squared residuals
-  for(unsigned int i=0; i < estimations.size(); ++i){
-
-    VectorXd residual = estimations[i] - ground_truth[i];
-
-    //coefficient-wise multiplication
-    residual = residual.array()*residual.array();
-    rmse += residual;
-  }
-
-  //calculate the mean
-  rmse = rmse/estimations.size();
-
-  //calculate the squared root
-  rmse = rmse.array().sqrt();
-
-  //return the result
-  return rmse;
-}
-
-MatrixXd EKFTools::CalculateJacobian(const VectorXd& x_state) {
-  /**
-    * Calculate a Jacobian here.
-  */
-  MatrixXd Hj(3,4);
-  Hj << 0,0,0,0,
-        0,0,0,0,
-        0,0,0,0;
-
-  //recover state parameters
-  float px = x_state(0);
-  float py = x_state(1);
-  float vx = x_state(2);
-  float vy = x_state(3);
-
-  //pre-compute a set of terms to avoid repeated calculation
-  float c1 = px*px+py*py;
-  float c2 = sqrt(c1);
-  float c3 = (c1*c2);
-
-  //check division by zero
-  if(fabs(c1) < 0.0001){
-    std::cout << "Function CalculateJacobian() has Error: Division by Zero" << std::endl;
-    return Hj;
-  }
-
-  //compute the Jacobian matrix
-  Hj << (px/c2),                (py/c2),                0,      0,
-        -(py/c1),               (px/c1),                0,      0,
-        py*(vx*py - vy*px)/c3,  px*(px*vy - py*vx)/c3,  px/c2,  py/c2;
-
-  return Hj;
-
-}

src/fusion/fusion_lidar_radar_camera/ekftools.h

@@ -1,30 +0,0 @@
-#ifndef EKFTOOLS_H_
-#define EKFTOOLS_H_
-#include <vector>
-#include "Eigen/Dense"
-
-class EKFTools {
-public:
-  /**
-  * Constructor.
-  */
-  EKFTools();
-
-  /**
-  * Destructor.
-  */
-  virtual ~EKFTools();
-
-  /**
-  * A helper method to calculate RMSE.
-  */
-  Eigen::VectorXd CalculateRMSE(const std::vector<Eigen::VectorXd> &estimations, const std::vector<Eigen::VectorXd> &ground_truth);
-
-  /**
-  * A helper method to calculate Jacobians.
-  */
-  Eigen::MatrixXd CalculateJacobian(const Eigen::VectorXd& x_state);
-
-};
-
-#endif /* EKFTOOLS_H_ */

src/fusion/fusion_lidar_radar_camera/fusion.hpp

@@ -1,343 +0,0 @@
-#ifndef FUSION_HPP
-#define FUSION_HPP
-#include "fusion_probabilities.h"
-#include "fusionobjectarray.pb.h"
-
-namespace iv {
-namespace fusion {
-
-// 先将毫米波和图像进行融合,将融合后的数据放入fusion中
-//void RadarCameraFusion(iv::vision::cameraobjectarray& camera_object_array, iv::radar::radarobjectarray& radar_object_array, iv::fusion::fusionobjectarray& Ra_Ca_fusion_object)
-//{
-//    for(int i = 0; i<camera_object_array.obj_size(); i++)
-//    {
-//        std::vector<int>  obj_index;
-
-//        for(int j = 0; j<radar_object_array.obj_size(); j++)
-//        {
-//            if(radar_object_array.obj(j).bvalid() == false) continue;
-//            if(!(FusionProbabilities::ComputeRadarCameraFusionProb(radar_object_array.obj(j),camera_object_array.obj(i)))) continue;
-////            else if () {
-
-////            }
-//            else {
-//                obj_index.push_back(j);
-//            }
-
-//         }
-//        if(obj_index.size()==0)
-//        {
-//            iv::fusion::fusionobject fusion_object;
-//            fusion_object.set_id(camera_object_array.obj(i).id());
-//            fusion_object.set_type(camera_object_array.obj(i).type());
-//            fusion_object.set_prob(camera_object_array.obj(i).con());
-
-//            iv::fusion::Dimension dimension;
-//            iv::fusion::Dimension *obj_dimension;
-//            dimension.set_x(camera_object_array.obj(i).w());
-//            dimension.set_z(camera_object_array.obj(i).h());
-//            obj_dimension = fusion_object.mutable_dimensions();
-//            obj_dimension->CopyFrom(dimension);
-
-//            iv::fusion::fusionobject *po = Ra_Ca_fusion_object.add_obj();
-//            po->CopyFrom(fusion_object);
-
-//        }else if (obj_index.size()==1)
-//        {
-//            iv::fusion::fusionobject fusion_object;
-//            fusion_object.set_id(camera_object_array.obj(i).id());
-//            fusion_object.set_type(camera_object_array.obj(i).type());
-//            fusion_object.set_prob(camera_object_array.obj(i).con());
-
-//            iv::fusion::PointXYZ centroid;
-//            iv::fusion::PointXYZ *centerpoint;
-//            centroid.set_x(radar_object_array.obj(obj_index.at(0)).x());
-//            centroid.set_y(radar_object_array.obj(obj_index.at(0)).y());
-//            centerpoint=fusion_object.mutable_centroid();
-//            centerpoint->CopyFrom(centroid);
-
-//            iv::fusion::Dimension dimension;
-//            iv::fusion::Dimension *obj_dimension;
-//            dimension.set_x(camera_object_array.obj(i).w());
-//            dimension.set_z(camera_object_array.obj(i).h());
-//            obj_dimension = fusion_object.mutable_dimensions();
-//            obj_dimension->CopyFrom(dimension);
-
-//            iv::fusion::VelXY vel_relative;
-//            iv::fusion::VelXY *velrelative;
-//            vel_relative.set_x(radar_object_array.obj(obj_index.at(0)).vx());
-//            vel_relative.set_y(radar_object_array.obj(obj_index.at(0)).vy());
-
-//            velrelative = fusion_object.mutable_vel_relative();
-//            velrelative->CopyFrom(vel_relative);
-
-//            iv::fusion::fusionobject *po = Ra_Ca_fusion_object.add_obj();
-//            po->CopyFrom(fusion_object);
-//        }else
-//        {
-
-
-//        }
-//        obj_index.clear();
-
-//    }
-//}
-
-float ComputeDis(cv::Point2f po1, cv::Point2f po2)
-{
-    return (sqrt(pow((po1.x-po2.x),2) + pow((po1.y-po2.y),2)));
-}
-
-
-void RLfusion( iv::lidar::objectarray& lidar_object_array,  iv::radar::radarobjectarray& radar_object_array, iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
-{
-    lidar_radar_fusion_object_array.Clear();
-    std::cout<<"~~~~~~~~~~~~"<<lidar_object_array.obj_size()<<std::endl;
-
-    for(int i = 0; i<lidar_object_array.obj_size(); i++)
-    {
-        std::vector<int> fuindex;
-        for(int j =0; j<radar_object_array.obj_size(); j++)
-        {
-            if(radar_object_array.obj(j).bvalid() == false) continue;
-            if(!(iv::fusion::FusionProbabilities::ComputRadarLidarmatch(radar_object_array.obj(j),lidar_object_array.obj(i))))
-            {
-                fuindex.push_back(j);
-            }
-        }
-        if(fuindex.size() == 0)
-        {
-            iv::fusion::fusionobject fusion_object;
-            fusion_object.set_id(lidar_object_array.obj(i).id());
-            fusion_object.set_type_name(lidar_object_array.obj(i).type_name());
-//            fusion_object.set_prob(lidar_object_array.obj(i).type_probs());
-            fusion_object.set_yaw(lidar_object_array.obj(i).tyaw());
-
-            Vel_Struct vels;
-            iv::fusion::VelXY vel;
-            iv::fusion::VelXY *vel_;
-            vels = iv::fusion::Transformation::lidarVelTra(lidar_object_array.obj(i).tyaw(),lidar_object_array.obj(i).velocity_linear_x());
-            vel.set_x(vels.vx);
-            vel.set_y(vels.vy);
-            vel_ = fusion_object.mutable_vel_relative();
-            vel_->CopyFrom(vel);
-
-            iv::fusion::PointXYZ centroid;
-            iv::fusion::PointXYZ *centroid_;
-            centroid.set_x(lidar_object_array.obj(i).centroid().x());
-            centroid.set_y(lidar_object_array.obj(i).centroid().y());
-            centroid.set_z(lidar_object_array.obj(i).centroid().z());
-            centroid_ = fusion_object.mutable_centroid();
-            centroid_->CopyFrom(centroid);
-
-
-
-            iv::fusion::Dimension dimension;
-            iv::fusion::Dimension *dimension_;
-            dimension.set_x(lidar_object_array.obj(i).dimensions().x());
-            dimension.set_y(lidar_object_array.obj(i).dimensions().y());
-            dimension.set_z(lidar_object_array.obj(i).dimensions().z());
-            dimension_ = fusion_object.mutable_dimensions();
-            dimension_->CopyFrom(dimension);
-
-
-            for(int k =0 ;k<lidar_object_array.obj(i).cloud().size(); k++)
-            {
-                iv::fusion::PointXYZI *pointcloud_ = fusion_object.add_cloud();
-                pointcloud_->set_x(lidar_object_array.obj(i).cloud(k).x());
-                pointcloud_->set_y(lidar_object_array.obj(i).cloud(k).y());
-                pointcloud_->set_z(lidar_object_array.obj(i).cloud(k).z());
-                pointcloud_->set_i(lidar_object_array.obj(i).cloud(k).i());
-            }
-
-                iv::fusion::fusionobject *pobj = lidar_radar_fusion_object_array.add_obj();
-                pobj->CopyFrom(fusion_object);
-            }else
-            {
-                std::vector<float> dis;
-                cv::Point2f po1;
-                po1.x = lidar_object_array.obj(i).centroid().x();
-                po1.y = lidar_object_array.obj(i).centroid().y();
-                for(std::vector<int>::iterator d = fuindex.begin(); d !=fuindex.end(); d++)
-                {
-                    cv::Point2f po2;
-                    po2.x = radar_object_array.obj(*d).x();
-                    po2.y = radar_object_array.obj(*d).y();
-                    dis.push_back(ComputeDis(po1,po2));
-                }
-                auto smallest = std::min_element(std::begin(dis), std::end(dis));
-                int index = std::distance(std::begin(dis), smallest);
-                dis.clear();
-
-                iv::fusion::fusionobject fusion_object;
-                fusion_object.set_id(lidar_object_array.obj(i).id());
-                fusion_object.set_type_name(lidar_object_array.obj(i).type_name());
-//                fusion_object.set_prob(lidar_object_array.obj(i).type_probs());
-                fusion_object.set_yaw(lidar_object_array.obj(i).tyaw());
-
-                iv::fusion::VelXY vel;
-                iv::fusion::VelXY *vel_;
-                vel.set_x(radar_object_array.obj(fuindex[index]).vx());
-                vel.set_y(radar_object_array.obj(fuindex[index]).vy());
-                vel_ = fusion_object.mutable_vel_relative();
-                vel_->CopyFrom(vel);
-
-                iv::fusion::PointXYZ centroid;
-                iv::fusion::PointXYZ *centroid_;
-                centroid.set_x(radar_object_array.obj(fuindex[index]).x());
-                centroid.set_y(radar_object_array.obj(fuindex[index]).y());
-                centroid.set_z(lidar_object_array.obj(i).centroid().z());
-                centroid_ = fusion_object.mutable_centroid();
-                centroid_->CopyFrom(centroid);
-
-                iv::fusion::Dimension dimension;
-                iv::fusion::Dimension *dimension_;
-                dimension.set_x(lidar_object_array.obj(i).dimensions().x());
-                dimension.set_y(lidar_object_array.obj(i).dimensions().y());
-                dimension.set_z(lidar_object_array.obj(i).dimensions().z());
-                dimension_ = fusion_object.mutable_dimensions();
-                dimension_->CopyFrom(dimension);
-
-                for(int k =0 ;k<lidar_object_array.obj(i).cloud().size(); k++)
-                {
-                    iv::fusion::PointXYZI *pointcloud_ = fusion_object.add_cloud();
-                    pointcloud_->set_x(lidar_object_array.obj(i).cloud(k).x());
-                    pointcloud_->set_y(lidar_object_array.obj(i).cloud(k).y());
-                    pointcloud_->set_z(lidar_object_array.obj(i).cloud(k).z());
-                    pointcloud_->set_i(lidar_object_array.obj(i).cloud(k).i());
-                }
-
-                iv::fusion::fusionobject *pobj = lidar_radar_fusion_object_array.add_obj();
-                pobj->CopyFrom(fusion_object);
-            }
-            fuindex.clear();
-           }
-        }
-
-
-
-void RLVfusion( iv::fusion::fusionobjectarray& li_ra_fusion_array, iv::vision::ObstacleInfo& vision_object_array,  iv::fusion::fusionobjectarray& li_ra_ca_fusion_array)
-{
-    li_ra_ca_fusion_array.Clear();
-    for(int i = 0; i<li_ra_fusion_array.obj_size();i++)
-    {
-        std::vector<int> findexs;
-        std::vector<int> flags;
-        for(int j = 0; j<vision_object_array.bbox_3d_size(); j++)
-        {
-            int result = iv::fusion::FusionProbabilities::ComputeLidarCameraFusionProb(li_ra_fusion_array.obj(i), vision_object_array.bbox_3d(j));
-            if(result <20) continue;
-            else {
-                findexs.push_back(j);
-                flags.push_back(result);
-            }
-        }
-
-        if(findexs.size() == 0)
-        {
-            iv::fusion::fusionobject fusionobject;
-            fusionobject.set_id(li_ra_fusion_array.obj(i).id());
-            fusionobject.set_yaw(li_ra_fusion_array.obj(i).yaw());
-            fusionobject.set_type_name(li_ra_fusion_array.obj(i).type_name());
-            fusionobject.set_prob(li_ra_fusion_array.obj(i).prob());
-
-            iv::fusion::VelXY vel;
-            iv::fusion::VelXY *vel_;
-            vel.set_x(li_ra_fusion_array.obj(i).vel_relative().x());
-            vel.set_y(li_ra_fusion_array.obj(i).vel_relative().y());
-            vel_ = fusionobject.mutable_vel_relative();
-            vel_->CopyFrom(vel);
-
-            iv::fusion::PointXYZ centroid;
-            iv::fusion::PointXYZ *centroid_;
-            centroid.set_x(li_ra_fusion_array.obj(i).centroid().x());
-            centroid.set_y(li_ra_fusion_array.obj(i).centroid().y());
-            centroid.set_z(li_ra_fusion_array.obj(i).centroid().z());
-            centroid_ = fusionobject.mutable_centroid();
-            centroid_->CopyFrom(centroid);
-
-            iv::fusion::Dimension dimension;
-            iv::fusion::Dimension *dimension_;
-            dimension.set_x(li_ra_fusion_array.obj(i).dimensions().x());
-            dimension.set_y(li_ra_fusion_array.obj(i).dimensions().y());
-            dimension.set_z(li_ra_fusion_array.obj(i).dimensions().z());
-            dimension_ = fusionobject.mutable_dimensions();
-            dimension_->CopyFrom(dimension);
-
-            for(int m =0 ;m<li_ra_fusion_array.obj(i).cloud().size(); m++)
-            {
-                iv::fusion::PointXYZI pointcloud;
-                iv::fusion::PointXYZI *pointcloud_;
-                pointcloud.set_x(li_ra_fusion_array.obj(i).cloud(m).x());
-                pointcloud.set_y(li_ra_fusion_array.obj(i).cloud(m).y());
-                pointcloud.set_z(li_ra_fusion_array.obj(i).cloud(m).z());
-                pointcloud.set_i(li_ra_fusion_array.obj(i).cloud(m).i());
-                pointcloud_= fusionobject.add_cloud();
-                pointcloud_->CopyFrom(pointcloud);
-           }
-
-           iv::fusion::fusionobject *pbj = li_ra_ca_fusion_array.add_obj();
-           pbj->CopyFrom(fusionobject);
-       }else
-        {
-            auto bigest = std::min_element(std::begin(flags), std::end(flags));
-            int index = std::distance(std::begin(flags), bigest);
-
-            iv::fusion::fusionobject fusionobject;
-            fusionobject.set_id(li_ra_fusion_array.obj(i).id());
-            fusionobject.set_yaw(li_ra_fusion_array.obj(i).yaw());
-//            fusionobject.set_prob(vision_object_array.bbox_3d(findexs[index]).);
-            fusionobject.set_type_name(vision_object_array.bbox_3d(findexs[index]).category());
-
-            iv::fusion::VelXY vel;
-            iv::fusion::VelXY *vel_;
-            vel.set_x(li_ra_fusion_array.obj(i).vel_relative().x());
-            vel.set_y(li_ra_fusion_array.obj(i).vel_relative().y());
-            vel_ = fusionobject.mutable_vel_relative();
-            vel_->CopyFrom(vel);
-
-            iv::fusion::PointXYZ centroid;
-            iv::fusion::PointXYZ *centroid_;
-            centroid.set_x(li_ra_fusion_array.obj(i).centroid().x());
-            centroid.set_y(li_ra_fusion_array.obj(i).centroid().y());
-            centroid.set_z(li_ra_fusion_array.obj(i).centroid().z());
-            centroid_ = fusionobject.mutable_centroid();
-            centroid_->CopyFrom(centroid);
-
-            iv::fusion::Dimension dimension;
-            iv::fusion::Dimension *dimension_;
-            dimension.set_x(li_ra_fusion_array.obj(i).dimensions().x());
-            dimension.set_y(li_ra_fusion_array.obj(i).dimensions().y());
-            dimension.set_z(li_ra_fusion_array.obj(i).dimensions().z());
-            dimension_ = fusionobject.mutable_dimensions();
-            dimension_->CopyFrom(dimension);
-
-            for(int n =0 ;n<li_ra_ca_fusion_array.obj(i).cloud().size(); n++)
-            {
-                iv::fusion::PointXYZI pointcloud;
-                iv::fusion::PointXYZI *pointcloud_;
-                pointcloud.set_x(li_ra_fusion_array.obj(i).cloud(n).x());
-                pointcloud.set_y(li_ra_fusion_array.obj(i).cloud(n).y());
-                pointcloud.set_z(li_ra_fusion_array.obj(i).cloud(n).z());
-                pointcloud.set_i(li_ra_fusion_array.obj(i).cloud(n).i());
-                pointcloud_= fusionobject.add_cloud();
-                pointcloud_->CopyFrom(pointcloud);
-            }
-
-            iv::fusion::fusionobject *pobj = li_ra_ca_fusion_array.add_obj();
-            pobj->CopyFrom(fusionobject);
-            }
-
-        findexs.clear();
-        flags.clear();
-
-    }
-
-}
-
-
-}
-}
-
-
-#endif // FUSION_HPP

src/fusion/fusion_lidar_radar_camera/fusion_probabilities.cpp

@@ -1,98 +0,0 @@
-#include "fusion_probabilities.h"
-
-
-int iv::fusion::FusionProbabilities::ComputePointInBox(cv::Point2f PointInCamera, Point top_left, Point bot_right)
-{
-    if(!((PointInCamera.x>=top_left.x)&&(PointInCamera.x<=bot_right.x)&&(PointInCamera.y>=bot_right.y)&&(PointInCamera.y<=top_left.y)))
-    {
-        return 0;
-    } else {
-        return 1;
-    }
-}
-
-int iv::fusion::FusionProbabilities::ComputeLidarCameraFusionProb(const iv::fusion::fusionobject& li_ra_fusion_object, const iv::vision::Bbox3D& vision_object )
-{
-    std::vector<cv::Point2f> lidarIncamera;
-    int flag = 0;
-    Point top_left, bot_right;
-    top_left.x = vision_object.top_left_x();
-    top_left.y = vision_object.top_left_y();
-    bot_right.x = vision_object.bottom_right_x();
-    bot_right.y = vision_object.bottom_right_y();
-
-    iv::fusion::Transformation::PointInCamera( li_ra_fusion_object,lidarIncamera);
-    for(std::vector<cv::Point2f>::iterator point2D = lidarIncamera.begin(); point2D != lidarIncamera.end(); point2D++)
-    {
-
-        flag = flag + ComputePointInBox(*point2D, top_left,bot_right);
-    }
-    lidarIncamera.clear();
-
-    return flag;
-}
-
-//int iv::fusion::FusionProbabilities::ComputeRadarInBox(Point radarincamera, Point top_left, Point bot_right)
-//{
-//    if(!((radarincamera.x>=top_left.x)&&(radarincamera.x<=bot_right.x)&&(radarincamera.y>=bot_right.y)&&(radarincamera.y<=top_left.y)))
-//    {
-//        return 0;
-//    } else {
-//        return 1;
-//    }
-//}
-
-
-//int iv::fusion::FusionProbabilities::ComputeRadarCameraFusionProb(const iv::radar::radarobject &radar_object, const iv::vision::cameraobject &camera_object)
-//{
-//    double z = 0.3;
-//    int flag = 0;
-//    Point top_left, bot_right;
-//    top_left.x = camera_object.x() - int(camera_object.w()/2);
-//    top_left.y = camera_object.y() + int(camera_object.h()/2);
-//    bot_right.x = camera_object.x() + int(camera_object.w()/2);
-//    bot_right.y = camera_object.y() - int(camera_object.h()/2);
-
-
-//    for(int i = 0; i<5; i++)
-//    {
-//        Point radar_in_world, radar_in_camera;
-//        radar_in_world.x = radar_object.x();
-//        radar_in_world.y = radar_object.y();
-//        radar_in_world.z = z*(i+1);
-
-//        radar_in_camera = iv::fusion::Transformation::RadarToCamera(radar_in_world);
-
-//        flag = flag + iv::fusion::FusionProbabilities::ComputeRadarInBox(radar_in_camera, top_left, bot_right);
-
-//    }
-
-//    if(flag>=3)
-//    {
-//        return 1;
-//    }else {
-//        return 0;
-//    }
-//}
-
-
-
-
-
-//毫米波雷达object点是否和激光雷达object的俯视box匹配
-int iv::fusion::FusionProbabilities::ComputRadarLidarmatch(const  iv::radar::radarobject& radarPoint, const  iv::lidar::object& lidarobject)
-{
-    if(!((radarPoint.x()>=lidarobject.max_point().x())&&(radarPoint.x()<= lidarobject.min_point().x())&&
-         ((radarPoint.y() - 2)>=lidarobject.max_point().y())&&((radarPoint.y() - 2)<=lidarobject.min_point().y())))
-    {
-        return 0;
-    } else {
-        return 1;
-    }
-}
-
-
-
-
-
-

src/fusion/fusion_lidar_radar_camera/fusion_probabilities.h

@@ -1,41 +0,0 @@
-#ifndef FUSION_PROBABILITIES_H
-#define FUSION_PROBABILITIES_H
-#include "transformation.h"
-
-//struct correct_box
-//{
-//    Point max_point;
-//    Point min_point;
-//};
-
-namespace iv {
-namespace fusion {
-
-
-class FusionProbabilities{
-public:
-    FusionProbabilities(){};
-    ~FusionProbabilities(){};
-
-    static int ComputePointInBox(cv::Point2f PointInCamera, Point top_left, Point bot_right);
-
-    static int ComputeRadarInBox(Point RadarInCamera, Point top_left, Point bot_right);
-    
-//    static int ComputeRadarLidarFusionProb(const iv::lidar::object& lidar_object, const iv::radar::radarobject& radar_object);
-
-    static int ComputeLidarCameraFusionProb(const iv::fusion::fusionobject& li_ra_fusion_object, const iv::vision::Bbox3D& vision_object );
-
-//    static int ComputeRadarCameraFusionProb(const iv::radar::radarobject &radar_object, const iv::vision::cameraobject &camera_object);
-
-//    static correct_box lidar_correct(double dt,const iv::lidar::object& lidar_object);
-
-    static int ComputRadarLidarmatch(const  iv::radar::radarobject& radarPoint, const  iv::lidar::object& lidarobject);
-    };
-
-
-   }
-}
-
-
-#endif // FUSION_PROBABILITIES_H
-

src/fusion/fusion_lidar_radar_camera/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/fusion_lidar_radar_camera/kalman_filter.cpp

@@ -1,115 +0,0 @@
-#include <math.h>
-#include "kalman_filter.h"
-
-using Eigen::MatrixXd;
-using Eigen::VectorXd;
-
-const float PI2 = 2 * M_PI;
-
-KalmanFilter::KalmanFilter() {}
-
-KalmanFilter::~KalmanFilter() {}
-
-void KalmanFilter::Init(VectorXd &x_in, MatrixXd &P_in, MatrixXd &F_in,
-                        MatrixXd &H_in, MatrixXd &R_in, MatrixXd &Q_in) {
-  x_ = x_in;
-  P_ = P_in;
-  F_ = F_in;
-  H_ = H_in;
-  R_ = R_in;
-  Q_ = Q_in;
-}
-
-void KalmanFilter::Predict() {
-  /**
-  TODO:
-    * predict the state
-  */
-  x_ = F_ * x_;
-  MatrixXd Ft = F_.transpose();
-  P_ = F_ * P_ * Ft + Q_;
-}
-
-void KalmanFilter::Update(const VectorXd &z) {
-  /**
-  TODO:
-    * update the state by using Kalman Filter equations
-  */
-
-  VectorXd z_pred = H_ * x_;
-
-  VectorXd y = z - z_pred;
-  MatrixXd Ht = H_.transpose();
-  MatrixXd PHt = P_ * Ht;
-  MatrixXd S = H_ * PHt + R_;
-  MatrixXd Si = S.inverse();
-  MatrixXd K = PHt * Si;
-
-  //new estimate
-  x_ = x_ + (K * y);
-  long x_size = x_.size();
-  MatrixXd I = MatrixXd::Identity(x_size, x_size);
-  P_ = (I - K * H_) * P_;
-
-}
-
-VectorXd RadarCartesianToPolar(const VectorXd &x_state)
-{
-  /*
-   * convert radar measurements from cartesian coordinates (x, y, vx, vy) to
-   * polar (rho, phi, rho_dot) coordinates
-  */
-  float px, py, vx, vy;
-  px = x_state[0];
-  py = x_state[1];
-  vx = x_state[2];
-  vy = x_state[3];
-
-  float rho, phi, rho_dot;
-  rho = sqrt(px*px + py*py);
-  phi = atan2(py, px);  // returns values between -pi and pi
-
-  // if rho is very small, set it to 0.0001 to avoid division by 0 in computing rho_dot
-  if(rho < 0.000001)
-    rho = 0.000001;
-
-  rho_dot = (px * vx + py * vy) / rho;
-
-  VectorXd z_pred = VectorXd(3);
-  z_pred << rho, phi, rho_dot;
-
-  return z_pred;
-
-}
-
-void KalmanFilter::UpdateEKF(const VectorXd &z) {
-  /**
-    * update the state by using Extended Kalman Filter equations
-  */
-
-  // convert radar measurements from cartesian coordinates (x, y, vx, vy) to polar (rho, phi, rho_dot).
-  VectorXd z_pred = RadarCartesianToPolar(x_);
-  VectorXd y = z - z_pred;
-
-  // normalize the angle between -pi to pi
-  while(y(1) > M_PI){
-    y(1) -= PI2;
-  }
-
-  while(y(1) < -M_PI){
-    y(1) += PI2;
-  }
-
-  // following is exact the same as in the function of KalmanFilter::Update()
-  MatrixXd Ht = H_.transpose();
-  MatrixXd PHt = P_ * Ht;
-  MatrixXd S = H_ * PHt + R_;
-  MatrixXd Si = S.inverse();
-  MatrixXd K = PHt * Si;
-
-  //new estimate
-  x_ = x_ + (K * y);
-  long x_size = x_.size();
-  MatrixXd I = MatrixXd::Identity(x_size, x_size);
-  P_ = (I - K * H_) * P_;
-}

src/fusion/fusion_lidar_radar_camera/kalman_filter.h

@@ -1,69 +0,0 @@
-#ifndef KALMAN_FILTER_H_
-#define KALMAN_FILTER_H_
-#include "Eigen/Dense"
-
-class KalmanFilter {
-public:
-
-  // state vector
-  Eigen::VectorXd x_;
-
-  // state covariance matrix
-  Eigen::MatrixXd P_;
-
-  // state transistion matrix
-  Eigen::MatrixXd F_;
-
-  // process covariance matrix
-  Eigen::MatrixXd Q_;
-
-  // measurement matrix
-  Eigen::MatrixXd H_;
-
-  // measurement covariance matrix
-  Eigen::MatrixXd R_;
-
-  /**
-   * Constructor
-   */
-  KalmanFilter();
-
-  /**
-   * Destructor
-   */
-  virtual ~KalmanFilter();
-
-  /**
-   * Init Initializes Kalman filter
-   * @param x_in Initial state
-   * @param P_in Initial state covariance
-   * @param F_in Transition matrix
-   * @param H_in Measurement matrix
-   * @param R_in Measurement covariance matrix
-   * @param Q_in Process covariance matrix
-   */
-  void Init(Eigen::VectorXd &x_in, Eigen::MatrixXd &P_in, Eigen::MatrixXd &F_in,
-      Eigen::MatrixXd &H_in, Eigen::MatrixXd &R_in, Eigen::MatrixXd &Q_in);
-
-  /**
-   * Prediction Predicts the state and the state covariance
-   * using the process model
-   * @param delta_T Time between k and k+1 in s
-   */
-  void Predict();
-
-  /**
-   * Updates the state by using standard Kalman Filter equations
-   * @param z The measurement at k+1
-   */
-  void Update(const Eigen::VectorXd &z);
-
-  /**
-   * Updates the state by using Extended Kalman Filter equations
-   * @param z The measurement at k+1
-   */
-  void UpdateEKF(const Eigen::VectorXd &z);
-
-};
-
-#endif /* KALMAN_FILTER_H_ */

src/fusion/fusion_lidar_radar_camera/lidar_radar_camera_fusion.pro

@@ -1,74 +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/cameraobject.pb.h \
-    ../../include/msgtype/cameraobjectarray.pb.h \
-    ../../include/msgtype/fusionobject.pb.h \
-    ../../include/msgtype/fusionobjectarray.pb.h \
-    ../../include/msgtype/object.pb.h \
-    ../../include/msgtype/objectarray.pb.h \
-    ../../include/msgtype/radarobject.pb.h \
-    ../../include/msgtype/radarobjectarray.pb.h \
-    ../../include/msgtype/rawpic.pb.h \
-    fusion.hpp \
-    fusion_probabilities.h \
-    imageBuffer.h \
-    transformation.h \
-    Tracker/Ctracker.h \
-    Tracker/defines.h \
-    Tracker/HungarianAlg.h \
-    Tracker/Kalman.h \
-    Tracker/ShortPathCalculator.h \
-    Tracker/track.h \
-    Tracker/VOTTracker.hpp \
-    Tracker/Tracking.h
-
-
-
-SOURCES += \
-        ../../include/msgtype/cameraobject.pb.cc \
-        ../../include/msgtype/cameraobjectarray.pb.cc \
-        ../../include/msgtype/fusionobject.pb.cc \
-        ../../include/msgtype/fusionobjectarray.pb.cc \
-        ../../include/msgtype/object.pb.cc \
-        ../../include/msgtype/objectarray.pb.cc \
-        ../../include/msgtype/radarobject.pb.cc \
-        ../../include/msgtype/radarobjectarray.pb.cc \
-        ../../include/msgtype/rawpic.pb.cc \
-        fusion_probabilities.cpp \
-        main.cpp \
-        transformation.cpp \
-    Tracker/Ctracker.cpp \
-    Tracker/HungarianAlg.cpp \
-    Tracker/Kalman.cpp \
-    Tracker/track.cpp
-
-INCLUDEPATH += /usr/include/eigen3
-INCLUDEPATH += $$PWD/../../include/msgtype
-
-LIBS += -lprotobuf
-
-INCLUDEPATH += $$PWD/../../../include/
-
-LIBS += -L$$PWD/../../../bin -lxmlparam -lmodulecomm -livlog -livfault -ldatamanager
-
-#INCLUDEPATH += /usr/include/
-
-LIBS += /usr/lib/libopencv*.so
-
-#INCLUDEPATH += /opt/ros/kinetic/include/opencv-3.3.1-dev
-
-#LIBS += /opt/ros/kinetic/lib/x86_64-linux-gnu/libopencv*.so
-
-

src/fusion/fusion_lidar_radar_camera/main.cpp

@@ -1,178 +0,0 @@
-#include <QCoreApplication>
-#include <QDateTime>
-#include <iostream>
-#include "modulecomm.h"
-//#include "cameraobjectarray.pb.h"
-#include "obstacles.pb.h"
-#include "objectarray.pb.h"
-#include "radarobjectarray.pb.h"
-#include "fusionobjectarray.pb.h"
-#include "rawpic.pb.h"
-#include "fusionobject.pb.h"
-#include <QThread>
-#include <QString>
-#include "opencv2/highgui.hpp"
-#include "opencv2/opencv.hpp"
-#include "imageBuffer.h"
-#include <QMutex>
-#include "eigen3/Eigen/Dense"
-#include "data_manager.h"
-#include "fusion.hpp"
-#include "Tracking.h"
-using namespace iv;
-using namespace fusion;
-
-void *gfu = iv::modulecomm::RegisterSend("li_ca_ra_fusion",10000000,1);
-static QMutex gMutex;
-
-typedef  iv::radar::radarobjectarray RadarDataType;
-typedef  iv::lidar::objectarray LidarDataType;
-//typedef  iv::vision::cameraobjectarray CameraDataType;
-typedef  iv::vision::ObstacleInfo VisionDataType;
-typedef  std::chrono::system_clock::time_point TimeType;
-typedef  std::function<void(RadarDataType&, LidarDataType&,VisionDataType&)> DataCallback;
-
-void data_callback(RadarDataType& radarobjv , LidarDataType& lidarobjv,VisionDataType& cameraobjv);
-
-QTime gTime;
-using namespace std;
-int gntemp = 0;
-
-
-//iv::vision::cameraobjectarray* cameraobjvec = new iv::vision::cameraobjectarray();
-//iv::radar::radarobjectarray* radarobjvec = new iv::radar::radarobjectarray();
-//iv::lidar::objectarray* lidarobjvec = new iv::lidar::objectarray();
-iv::fusion::fusionobjectarray li_ra_fusion,li_ra_ca_fusion;
-ConsumerProducerQueue<cv::Mat> *imageBuffer = new ConsumerProducerQueue<cv::Mat>(1, true);
-
-TrackerSettings settings;
-CTracker tracker(settings);
-bool m_isTrackerInitialized = false;
-iv::DataManager data_manager;
-void datafusion(RadarDataType& radarobjvec , LidarDataType& lidarobjvec,VisionDataType& visionobjv);
-
-
-void Listenesrfront(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-
-    iv::radar::radarobjectarray radarobj;
-    if(nSize<1)return;
-    if(false == radarobj.ParseFromArray(strdata,nSize))
-    {
-        std::cout<<"PecerptionShow Listenesrfront fail."<<std::endl;
-        return;
-    }
-    else{
-        //std::cout<<"srfront byte size:  "<<radarobjvec.ByteSize()<<std::endl;
-    }
-    gMutex.lock();
-//    radarobjvec->CopyFrom(radarobj);
-//            std::cout<<"11111111111111111111111"<<std::endl;
-    data_manager.SetRadar(radarobj);
-    gMutex.unlock();
-}
-
-void Listencamera(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-
-
-    if(nSize<1)return;
-    iv::vision::ObstacleInfo visionobj;
-    if(false == visionobj.ParseFromArray(strdata, nSize))
-    {
-//        std::cout<<"ccccccccccc Listencamera fail."<<std::endl;
-        return;
-    }
-    else{
-        //std::cout<<"camera byte size:  "<<cameraobjvec.ByteSize()<<std::endl;
-    }
-    gMutex.lock();
-//    std::cout<<"x:  "<<cameraobj.obj(0).x()<<"      y: "<<cameraobj.obj(0).y()<<std::endl;
-
-//    cameraobjvec->CopyFrom(cameraobj);
-//            std::cout<<"222222222222222"<<std::endl;
-    data_manager.SetCamera(visionobj);
-    gMutex.unlock();
-}
-
-void Listenlidarcnndetect(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-
-
-    //    std::vector<iv::lidar::object>  lidarobjvec;
-    //    strtolidarobj(lidarobjvec,strdata,nSize);
-    iv::lidar::objectarray lidarobj;
-    if(nSize<1)return;
-    if(false == lidarobj.ParseFromArray(strdata,nSize))
-    {
-        std::cout<<"PecerptionShow Listenesrfront fail."<<std::endl;
-        return;
-    }
-    gMutex.lock();
-//    std::cout<<"x: "<<lidarobj.obj(0).centroid().x()<<"          y: "<<lidarobj.obj(0).centroid().y()<<std::endl;
-//    lidarobjvec->CopyFrom(lidarobj);
-//            std::cout<<"3333333333333"<<std::endl;
-    data_manager.SetLidar(lidarobj);
-    gMutex.unlock();
-}
-
-void data_callback(RadarDataType& radarobjv , LidarDataType& lidarobjv,VisionDataType& visionobjv)
-{
-    if((radarobjv.ByteSize() == 0) || (visionobjv.ByteSize() ==0) || (lidarobjv.ByteSize() ==0)) return;
-//    gMutex.lock();
-    datafusion(radarobjv,lidarobjv,visionobjv);
-//    gMutex.unlock();
-}
-
-
-
-int ccccc =0;
-void datafusion(RadarDataType& radarobjvec , LidarDataType& lidarobjvec,VisionDataType& visionobjv)
-{
-    gMutex.lock();
-    Transformation::RadarPross(radarobjvec);
-    gMutex.unlock();
-
-    gMutex.lock();
-    RLfusion(lidarobjvec,radarobjvec,li_ra_fusion);
-    gMutex.unlock();
-
-    gMutex.lock();
-    //std::cout<<"RLVfusion  begin:  "<<std::endl;
-    RLVfusion(li_ra_fusion,visionobjv,li_ra_ca_fusion);
-   // std::cout<<"RLVfusion  end:  "<<std::endl;
-    //---------------------------------------------  init tracker  -------------------------------------------------
-    if (!m_isTrackerInitialized)
-    {
-        m_isTrackerInitialized = InitTracker(tracker);
-        if (!m_isTrackerInitialized)
-        {
-            std::cerr << "Tracker initialize error!!!" << std::endl;
-        }
-    }
-    Tracking(li_ra_ca_fusion, tracker);
-    //--------------------------------------------  end tracking  ---------------------------------------------------
-    gMutex.unlock();
-
-    std::string out = li_ra_ca_fusion.SerializeAsString();
-    iv::modulecomm::ModuleSendMsg(gfu,out.data(),out.length());
-    std::cout<<li_ra_fusion.obj_size()<<"~~~~~~~~~~~"<<std::endl;
-    std::cout<<li_ra_ca_fusion.obj_size()<<"----------"<<std::endl;
-    qDebug("lenth is %d",out.length());
-}
-
-
-int main(int argc, char *argv[])
-{
-    QCoreApplication a(argc, argv);
-    tracker.setSettings(settings);
-    data_manager.SetDataCallback(data_callback);
-    data_manager.Ready();
-
-    void *gpa;
-//    gpa = iv::modulecomm::RegisterRecv("camera_detect",Listendetection);
-    gpa = iv::modulecomm::RegisterRecv("radar_esr_f",Listenesrfront);
-    gpa = iv::modulecomm::RegisterRecv("vision_obstacles",Listencamera);
-    gpa = iv::modulecomm::RegisterRecv("lidar_track",Listenlidarcnndetect);
-    return a.exec();
-}

src/fusion/fusion_lidar_radar_camera/transformation.cpp

@@ -1,234 +0,0 @@
-#include "transformation.h"
-
-using namespace Eigen;
-using namespace std;
-//相机坐标系转换到物理坐标系
-//Point iv::fusion::Transformation::CameraToWorld(Point CameraInfo)
-//{
-//    Eigen::Matrix<double, 3,3> M_mat;
-//    M_mat <<
-//             -4.5288434998620575e-01, -2.7615788020977561e-01,7.9366326248964913e+02,
-//             -9.4286502898149827e-02,-1.7340325696936468e-01, 1.8919238200384973e+02,
-//             -1.2880669794829137e-04, -3.2569693624068995e-04, 1. ;
-
-//    Point camerainworld;
-//    Eigen::MatrixXd camerapoint(3,1);
-//    camerapoint(0,0) = CameraInfo.x;
-//    camerapoint(1,0) = CameraInfo.y;
-//    camerapoint(2,0) = 1;
-
-//    MatrixXd m2 = M_mat * camerapoint;
-
-//    MatrixXd retMat(2, 1);
-//    retMat(0, 0) = m2(0, 0) / m2(2, 0);
-//    retMat(1, 0) = m2(1, 0) / m2(2, 0);
-
-//    camerainworld.x = retMat(0,0);
-//    camerainworld.y = retMat(1,0);
-
-//    return camerainworld;
-//}
-
-cv::Point3f iv::fusion::Transformation::CameraToWorld(const iv::vision::Bbox3D& obs)
-{
-   cv::Point3f pointInWord;
-   Eigen::Matrix<float, 4,4> TR_mat;
-   TR_mat <<
-             5.9291132376544176e-01, 7.8826308046136040e-01, 1.6461311652562305e-01, -9.6543865533233912e+00,
-             2.6858663540518707e-01,-3.8629650733670828e-01, 8.8240366482760124e-01,6.4911669006353305e+00,
-             7.5915570302305757e-01, -4.7897424189723109e-01,-4.4075650212621942e-01,2.6308605270778315e+01,
-             0,0,0,1;
-   Eigen::Matrix<float, 4,1> point_camera_word;
-   point_camera_word << obs.x(),obs.y(),obs.z(),1;
-
-   Eigen::Matrix<float,4,1> point_word;
-
-   point_word = TR_mat.transpose()*point_camera_word;
-   pointInWord.x = point_word[0];
-   pointInWord.y = point_word[1];
-   pointInWord.z = point_word[2];
-
-   return pointInWord;
-}
-
-
-
-void iv::fusion::Transformation::LidarToCamera(std::vector<cv::Point3f> lidarpoint, std::vector<cv::Point2f>& camerapoint)
-{
-//        std::cout<<"LidarToCamera     begin:  "<<std::endl;
-
-        std::vector<cv::Point2f> camerapo;
-
-        cv::Mat K_mat = (cv::Mat_<double>(3,3)<<
-																			
-                             1053.864895, 0.000000, 971.348303,
-                                             0.000000, 968.877713, 550.964881,
-                                             0.000000, 0.000000, 1.000000
-						);
-        cv::Mat r_mat = (cv::Mat_<double>(3,1)<<1.1149181452320696e+00, -1.2470711778466399e+00,
-                         1.3130260558278872e+00 );
-
-        cv::Mat t_mat  = (cv::Mat_<double>(3,1)<<2.5470560990340907e-01, -1.0579365263005978e+00,
-                          -7.1889222854559742e-01);
-        cv::Mat distCoeffs = (cv::Mat_<double>(1,5)<<-2.9078999999999999e-01, 6.0994000000000000e-02,
-                                                      3.6489999999999999e-03, -2.0590000000000001e-03, 0. );
-
-//        std::vector<cv::Point3f> ot;
-//        ot.push_back(lidarpoint[0]);
-
-//        std::cout<<"ooppo"<<lidarpoint[0]<<std::endl;
-        cv::projectPoints(lidarpoint,r_mat,t_mat,K_mat,distCoeffs,camerapo);
-
-        for (vector<cv::Point2f>::iterator point = camerapo.begin(); point != camerapo.end(); point++) {
-            if(!(point->x > 1920)&&(point->y>1080) )
-            {
-                camerapoint.push_back(*point);
-
-            }
-
-        }
-        camerapo.clear();
-//        std::cout<<"LidarToCamera     end:  "<<std::endl;
-}
-
-//将激光聚类后的点云投射到图像上
-//输入激光雷达目标检测之后的数据，输出激光雷达点云在图像上成的点集
-void iv::fusion::Transformation::PointInCamera(const iv::fusion::fusionobject& li_ra_fusion_obj,std::vector<cv::Point2f>& lidarIn2D)
-{
-//    Point PointIncamera, PointInlidar;
-//    std::cout<<"PointInCamera     begin: "<<std::endl;
-        lidarIn2D.clear();
-        vector<cv::Point3f> lidarIn3D;
-        for(int j=0; j<li_ra_fusion_obj.cloud().size(); j++)
-        {
-            cv::Point3f PointInlidar;
-            if(!(li_ra_fusion_obj.cloud(j).x()>0.5 || li_ra_fusion_obj.cloud(j).y()<100)) continue;
-            PointInlidar.x = -li_ra_fusion_obj.cloud(j).y();
-            PointInlidar.y = li_ra_fusion_obj.cloud(j).x();
-            PointInlidar.z = li_ra_fusion_obj.cloud(j).z();
-
-            lidarIn3D.push_back(PointInlidar);
-        }
-//        std::cout<<"-----"<<typeid (lidarIn3D).name()<<std::endl;
-//        std::cout<<lidarIn3D<<std::endl;
-
-        LidarToCamera(lidarIn3D,lidarIn2D);
-
-        lidarIn3D.clear();
-
-//       std::cout<<"PointInCamera     end:  "<<std::endl;
-
-}
-
-//计算毫米波目标距离
-float iv::fusion::Transformation::ComputelonDistance(double x, double y)
-{
-    return (sqrt(pow(x,2) + pow(y,2)));
-}
-
-//计算毫米波目标速度
-float iv::fusion::Transformation::ComputeRadarSpeed(double vx, double vy)
-{
-    return (sqrt(pow(vx,2) + pow(vy,2)));
-}
-
-//毫米波数据去重
-//将速度相近、目标距离相近、纵向坐标相近的目标状态置为false
-void iv::fusion::Transformation::RadarPross(iv::radar::radarobjectarray& radar_object)
-{
-    for(int r = 0; r<(radar_object.obj_size()-1);r++)
-    {
-        if(radar_object.obj(r).bvalid() == false) continue;
-        for (int s=(r+1); s<(radar_object.obj_size());s++)
-        {
-            if(radar_object.obj(s).bvalid() == false) continue;
-            float lonDistance1 = ComputelonDistance(radar_object.obj(r).x(),radar_object.obj(r).y());
-            float lonDistance2 = ComputelonDistance(radar_object.obj(s).x(),radar_object.obj(s).y());
-            float RadarSpeed1 = ComputeRadarSpeed(radar_object.obj(r).vx(),radar_object.obj(r).vy());
-            float RadarSpeeds = ComputeRadarSpeed(radar_object.obj(s).vx(), radar_object.obj(s).vy());
-
-            if(
-                   (fabs(lonDistance1- lonDistance2) <= 1.00) &&
-                   (fabs(radar_object.obj(r).x() - radar_object.obj(s).x()) <=1.00)&&
-                   (fabs(RadarSpeed1-RadarSpeeds)<=1.00)
-             )
-            {
-//                radar_object.mutable_obj(s)->set_bvalid(false);
-                iv::radar::radarobject & radar_objct_new = (iv::radar::radarobject&)radar_object.obj(r);
-                radar_objct_new.set_bvalid(false);
-            }
-
-        }
-
-    }
-}
-
-
-Vel_Struct iv::fusion::Transformation::lidarVelTra(float yaw, float LineVel)
-{
-    Vel_Struct vel;
-
-    vel.vx = LineVel*sin(yaw);
-    vel.vy = LineVel*cos(yaw);
-
-    return vel;
-}
-
-////将毫米波目标投射到图像坐标系上
-////输入毫米波坐标系下坐标，输出图像坐标系下毫米波坐标
-//Point iv::fusion::Transformation::RadarToCamera(Point radarinfo){
-//    Point radarInCamera;
-
-
-////    Eigen::Matrix<double, 3,4> K_mat;
-
-
-//    Eigen::Matrix<double, 3,4> K_mat;
-
-//    Eigen::Matrix<double ,4,4> Trans_mat;
-
-
-
-//    K_mat <<
-//             -1.0770182913501825e-01, -9.9417648597809594e-01,
-//                    -3.6647959310434075e-03, 5.4851191193493243e-02,
-//                    -2.2614740047378623e-03, -9.9849197921664889e-01,
-//                    9.9266895933419130e-01, -1.0774043096057656e-01,
-//                    5.4775329399311401e-02;
-
-
-
-////    Eigen::Matrix<double ,4,4> Trans_mat;
-
-////    Eigen::Matrix<double, 3,3> M_mat;
-//    Trans_mat <<  1,0,0,0,
-//            0,1,0,1.252,
-//            0,0,1,-2.5,
-//            0,0,0,1;
-////    //translation
-////    double x = radarinfo.x - param.x;
-////    double y = radarinfo.y - param.y;
-////    double z = radarinfo.z - param.z;
-
-////    //rotate
-////    double yawAngle = param.yaw * M_PI/180;
-////    double x1 = x*cos( yawAngle ) - y*sin( yawAngle );
-////    double y1 = x*sin( yawAngle ) - y*cos( yawAngle);
-////    double z1 = z;
-
-////    double pitchAngle = param.pitch * M_PI/180;
-////    double x2 = x1;
-////    double y2 = y1 * cos( pitchAngle ) - z1 * sin(pitchAngle);
-////    double z2 = y1 * sin( pitchAngle ) + z1 * cos(pitchAngle);
-
-//    Eigen::Matrix<double, 4,1> real_position;
-//    real_position<< radarinfo.x, radarinfo.y, radarinfo.z, 1;
-
-//    Eigen::Matrix<double,3,1> uv_mat =K_mat*(Trans_mat*real_position);
-
-//    radarInCamera.x = int(fabs(uv_mat[0]/uv_mat[2]));
-//    radarInCamera.y = int(fabs(uv_mat[1]/uv_mat[2]));
-
-//    return radarInCamera;
-
-//}

src/fusion/fusion_lidar_radar_camera/transformation.h

@@ -1,56 +0,0 @@
-#ifndef TRANSFORMATION_H
-#define TRANSFORMATION_H
-#include "eigen3/Eigen/Dense"
-#include "objectarray.pb.h"
-//#include "cameraobjectarray.pb.h"
-#include "obstacles.pb.h"
-#include "radarobjectarray.pb.h"
-#include "fusionobjectarray.pb.h"
-#include "opencv2/opencv.hpp"
-struct Point{
-    double x;
-    double y;
-    double z;
-    double i;   //强度信息
-    double r;
-    double g;
-    double b;
-};
-
-struct Vel_Struct{
-    double vx;
-    double vy;
-};
-
-
-namespace iv {
-namespace fusion {
-
-
-class Transformation{
-public:
-    Transformation(){};
-    ~Transformation(){};
-
-    static Point RadarToCamera(Point radarInfo);
-
-//    static Point CameraToWorld(Point CameraInfo);
-    static cv::Point3f CameraToWorld(const iv::vision::Bbox3D& obs);
-
-    static void RadarPross(iv::radar::radarobjectarray& radar_object);
-
-    static float ComputelonDistance(double x, double y);
-
-    static float ComputeRadarSpeed(double vx, double vy);
-
-    static void LidarToCamera(std::vector<cv::Point3f> lidarpoint, std::vector<cv::Point2f>& camerapoint);  //激光雷达解析数据中 坐标系为  车正前方X轴、左边为Y轴、上方为Z轴。
-
-    static void PointInCamera(const iv::fusion::fusionobject& li_ra_fusion_obj,std::vector<cv::Point2f>& lidarIn2D);
-
-    static Vel_Struct lidarVelTra(float yaw, float LineVel);
-
-
-    };
-}
-}
-#endif // TRANSFORMATION_H

src/fusion/fusion_radar_camer/.qmake.stash

@@ -1,24 +0,0 @@
-QMAKE_CXX.QT_COMPILER_STDCXX = 201402L
-QMAKE_CXX.QMAKE_GCC_MAJOR_VERSION = 7
-QMAKE_CXX.QMAKE_GCC_MINOR_VERSION = 5
-QMAKE_CXX.QMAKE_GCC_PATCH_VERSION = 0
-QMAKE_CXX.COMPILER_MACROS = \
-    QT_COMPILER_STDCXX \
-    QMAKE_GCC_MAJOR_VERSION \
-    QMAKE_GCC_MINOR_VERSION \
-    QMAKE_GCC_PATCH_VERSION
-QMAKE_CXX.INCDIRS = \
-    /usr/include/c++/7 \
-    /usr/include/x86_64-linux-gnu/c++/7 \
-    /usr/include/c++/7/backward \
-    /usr/lib/gcc/x86_64-linux-gnu/7/include \
-    /usr/local/include \
-    /usr/lib/gcc/x86_64-linux-gnu/7/include-fixed \
-    /usr/include/x86_64-linux-gnu \
-    /usr/include
-QMAKE_CXX.LIBDIRS = \
-    /usr/lib/gcc/x86_64-linux-gnu/7 \
-    /usr/lib/x86_64-linux-gnu \
-    /usr/lib \
-    /lib/x86_64-linux-gnu \
-    /lib

src/fusion/fusion_radar_camer/fusion.cpp

@@ -1,353 +0,0 @@
-#include "eigen3/Eigen/Eigen"
-#include "fusion.h"
-#include <iostream>
-
-using namespace Eigen;
-using namespace std;
-
-//计算图像目标在世界坐标系下的坐标，输入图像坐标系坐标，输出世界坐标系坐标
-//float ComputeDis()
-//{
-
-//}
-
-//Point iv::fusion::Objectfusion::CameraToWorld(Point CameraInfo)
-//{
-//    Eigen::Matrix<float, 3,3> M_mat;
-//    M_mat <<
-
-//             -1.779425393883217, 0.08238080527237265, 1497.921944186997,
-//              -2.753065043204539e-14, -7.331891669241049, 2354.984038719003,
-//              -3.021031881519582e-18, -0.001853568118628354, 1.;
-
-
-////             -2.7341490227041249e-01, -1.4344331689120825e-02,2.4706078228937577e+02,
-////             9.3772377361721504e-02,-6.2813369063524449e-01, -3.2874944247737361e+01,
-////             1.0414363407561230e-04, -2.1564718162646128e-03, 1.;
-
-//    Point camerainworld;
-//    Eigen::MatrixXf camerapoint(3,1);
-//    camerapoint(0,0) = CameraInfo.x;
-//    camerapoint(1,0) = CameraInfo.y;
-//    camerapoint(2,0) = 1;
-
-//    MatrixXf m2 = M_mat * camerapoint;
-//    assert(m2(2, 0) != 0);
-
-//    camerainworld.x = (int(m2(0, 0) / m2(2, 0)))/1000;
-//    camerainworld.y = (int(m2(1, 0) / m2(2, 0)))/1000;
-
-//    std::cout<<CameraInfo.x<<"@@@@@@@@@@@@@"<<CameraInfo.y<<std::endl;
-
-//    std::cout<<camerainworld.x<<"~~~~~zzzzzzz~~~~~~~~"<<camerainworld.y<<std::endl;
-
-//    return camerainworld;
-//}
-
-cv::Point3f iv::fusion::Objectfusion::ComputeCameratToWord(const iv::vision::Bbox3D& obs)
-{
-   cv::Point3f pointInWord;
-   Eigen::Matrix<float, 4,4> TR_mat;
-   TR_mat <<
-             5.9291132376544176e-01, 7.8826308046136040e-01, 1.6461311652562305e-01, -9.6543865533233912e+00,
-             2.6858663540518707e-01,-3.8629650733670828e-01, 8.8240366482760124e-01,6.4911669006353305e+00,
-             7.5915570302305757e-01, -4.7897424189723109e-01,-4.4075650212621942e-01,2.6308605270778315e+01,
-             0,0,0,1;
-   Eigen::Matrix<float, 4,1> point_camera_word;
-   point_camera_word << obs.x(),obs.y(),obs.z(),1;
-
-
-   Eigen::Matrix<float,4,1> point_word;
-
-   point_word = TR_mat.transpose()*point_camera_word;
-   pointInWord.x = point_word[0];
-   pointInWord.y = point_word[1];
-   pointInWord.z = point_word[2];
-
-   return pointInWord;
-}
-
-
-
-
-
-//计算毫米波目标距离
-float iv::fusion::Objectfusion::ComputelonDistance(double x, double y)
-{
-    return (sqrt(pow(x,2) + pow(y,2)));
-
-}
-
-//计算毫米波目标速度
-float iv::fusion::Objectfusion::ComputeRadarSpeed(double vx, double vy)
-{
-    return (sqrt(pow(vx,2) + pow(vy,2)));
-}
-
-//毫米波数据去重
-//将速度相近、目标距离相近、纵向坐标相近的目标状态置为false
-void iv::fusion::Objectfusion::RadarPross(iv::radar::radarobjectarray& radar_object)
-{
-
-    for(int r = 0; r<(radar_object.obj_size()-1);r++)
-    {
-//        std::cout<<"r  :    "<<r<<"      size     :"<<radar_object.obj_size()<<std::endl;
-
-
-        if(radar_object.obj(r).bvalid() == false) continue;
-
-        if(radar_object.obj(r).y()>80||(fabs(radar_object.obj(r).x())>15))
-        {
-//            std::cout<<"RadarPross   begin: "<<std::endl;
-            iv::radar::radarobject & radar_objct_ = (iv::radar::radarobject&)radar_object.obj(r);
-            radar_objct_.set_bvalid(false);
-//            radar_object.mutable_obj(r)->set_bvalid(false);
-
-//            std::cout<<"RadarPross   end: "<<std::endl;
-        }
-
-        for (int s=(r+1); s<(radar_object.obj_size());s++)
-        {
-
-            if(radar_object.obj(s).bvalid() == false) continue;
-            float lonDistance1 = ComputelonDistance(radar_object.obj(r).x(),radar_object.obj(r).y());
-            float lonDistance2 = ComputelonDistance(radar_object.obj(s).x(),radar_object.obj(s).y());
-            float RadarSpeed1 = ComputeRadarSpeed(radar_object.obj(r).vx(),radar_object.obj(r).vy());
-            float RadarSpeeds = ComputeRadarSpeed(radar_object.obj(s).vx(), radar_object.obj(s).vy());
-//            std::cout<<"datafusion begin:"<<std::endl;
-            if(
-
-                   (fabs(radar_object.obj(r).x() - radar_object.obj(s).x()) <=0.5)||
-                    ((fabs(lonDistance1- lonDistance2) <= 0.50)&&(fabs(RadarSpeed1-RadarSpeeds)<=0.50))
-             ){
-//                radar_object.mutable_obj(s)->set_bvalid(false);
-                iv::radar::radarobject & radar_objct_new = (iv::radar::radarobject&)radar_object.obj(s);
-                radar_objct_new.set_bvalid(false);
-            }
-        }
-
-    }
-
-
-}
-
-//将毫米波目标投射到图像坐标系上
-//输入毫米波坐标系下坐标，输出图像坐标系下毫米波坐标
-//void iv::fusion::Objectfusion::RadarToCamera(Point& radarinfo, Point& pointIncamera){
-
-////    Eigen::Matrix<double, 3,4> K_mat;
-
-
-//    Eigen::Matrix<double, 3,4> K_mat;
-
-//    Eigen::Matrix<double ,4,4> Trans_mat;
-//    Eigen::Matrix<double, 1,5> distCoeffs;
-
-//    K_mat <<
-//             1053.864895, 0.000000, 971.348303,0.,
-//             0.000000, 968.877713, 550.964881,0.,
-//             0.000000, 0.000000, 1.000000,0.;
-
-//    distCoeffs<<-0.290790, 0.060994, 0.003649, -0.002059, 0.000000;
-
-
-//    Trans_mat <<
-//                 9.9729820313151096e-01,   -7.1503475479664416e-02,  1.6838854622221750e-02,     2.5119309561071688e+01,
-//                 1.3199346311828775e-02,   -5.1070628904529507e-02,  -9.9860781497053985e-01,    1.5333369174382897e+03,
-//                 7.2263900307136072e-02,   9.9613204137685718e-01,   -4.9988847303928038e-02,    -2.4780658279687159e+03,
-//            0.,0.,0.,1.;
-
-//    Eigen::Matrix<double, 4,1> real_position;
-//    real_position<< (radarinfo.x*1000), (radarinfo.y*1000), (radarinfo.z*1000), 1;
-
-//    Eigen::Matrix<double,3,1> uv_mat =K_mat*(Trans_mat*real_position);
-
-//    pointIncamera.x = int(fabs(uv_mat[0]/uv_mat[2]));
-//    pointIncamera.y = int(fabs(uv_mat[1]/uv_mat[2]));
-
-
-//}
-void iv::fusion::Objectfusion::RadarToCamera(std::vector<cv::Point3f>& radarpoint, std::vector<cv::Point2f>& camerapoint)
-{
-
-    cv::Mat K_mat = (cv::Mat_<double>(3,3)<<1.3045004012660161e+03, 0., 5.8482348464526285e+02, 0.,
-                     1.2112526100577470e+03, 1.8679663217256709e+02, 0., 0., 1. );
-    cv::Mat r_mat = (cv::Mat_<double>(3,1)<<-1.9340161235486149e+00, -8.4462583294643478e-01,
-                     -7.3826864590818786e-01 );
-
-    cv::Mat t_mat  = (cv::Mat_<double>(3,1)<<-9.6543865533233912e+00, 6.4911669006353305e+00,
-                      2.6308605270778315e+01);
-    cv::Mat distCoeffs = (cv::Mat_<double>(1,14)<<-9.6729350187520047e-02, -2.1731738003015462e+00, 0., 0., 0.,
-                          0., 0., -4.7445499728170173e+00, 0., 0., 0., 0., 0., 0.);
-
-    cv::projectPoints(radarpoint,r_mat,t_mat,K_mat,distCoeffs,camerapoint);
-
-}
-
-// 计算毫米波目标是否在bbox里面，是返回1,否返回0
-
-int iv::fusion::Objectfusion::Computeradartobox(cv::Point2f radarincamera,Point top_left,Point bot_right)
-{
-    if((radarincamera.x>=(top_left.x))&&(radarincamera.x<=(bot_right.x+5))&&(radarincamera.y<=(bot_right.y+5))&&(radarincamera.y>=(top_left.y)))
-    {
-        return 0;
-    } else {
-        return 1;
-    }
-}
-
-
-
-int iv::fusion::Objectfusion::Computefusionprobabilities(const  iv::radar::radarobject& radar_object, const  iv::vision::Bbox3D& vision_object )
-{
-//    fusion_state fu;
-    std::vector<cv::Point3f> radar_in_word;
-    std::vector<cv::Point2f> radar_in_camera;
-
-    double z = 0.3;
-    int flag = 0;
-    Point top_left, bot_right;
-    top_left.x = vision_object.top_left_x();
-    top_left.y = vision_object.top_left_y();
-    bot_right.x = vision_object.bottom_right_x();
-    bot_right.y = vision_object.bottom_right_y();
-
-    cv::Point3f radarobj;
-    radarobj.x = radar_object.x();
-    radarobj.y = radar_object.y();
-    radarobj.z = 0.5;
-    radar_in_word.push_back(radarobj);
-
-    RadarToCamera(radar_in_word,radar_in_camera);
-
-//    std::cout<<"-------------------------------------------------------"<<std::endl;
-//    std::cout<<"x1  :"<<radar_in_word[0].x<<"   y1 : "<<radar_in_word[0].y<< "    z1 : "<<radar_in_word[0].z<<std::endl;
-
-//    std::cout<<"x2  :"<<radar_in_camera[0].x<<"  y2 :" <<radar_in_camera[0].y<<std::endl;
-
-
-    int result = Computeradartobox(radar_in_camera[0], top_left, bot_right);
-    radar_in_word.clear();
-    radar_in_camera.clear();
-//    std::cout<<camera_object.w()<<std::endl;
-//    std::cout<<result<<"ooooooooooooo"<<std::endl;
-    return result;
-}
-
-//计算毫米波目标距离
-float iv::fusion::Objectfusion::ComputeDis(cv::Point2f po1, cv::Point2f po2)
-{
-    return (sqrt(pow((po1.x-po2.x),2) + pow((po1.y-po2.y),2)));
-}
-
-
-
-//将匹配上的目标有效值放入fusion里面
-void iv::fusion::Objectfusion::datafusion( iv::vision::ObstacleInfo& vision_object_array, iv::radar::radarobjectarray& radar_object_array, iv::fusion::fusionobjectarray& Ra_Ca_fusion_object_array)
-{
-    Ra_Ca_fusion_object_array.Clear();
-    for(int i = 0; i<vision_object_array.bbox_3d_size(); i++)
-    {
-        std::vector<int> fusionindex;
-        for(int j = 0; j<radar_object_array.obj_size(); j++)
-        {
-            if(radar_object_array.obj(j).bvalid() == false) continue;
-            int result = iv::fusion::Objectfusion::Computefusionprobabilities(radar_object_array.obj(j),vision_object_array.bbox_3d(i));
-            fusionindex.push_back(j);
-        }
-        if(fusionindex.size()==0)
-        {
-            cv::Point3f point_word;
-            iv::fusion::fusionobject fusion_object;
-            fusion_object.set_id(vision_object_array.bbox_3d(i).class_id());
-            fusion_object.set_type_name(vision_object_array.bbox_3d(i).category());
-//            fusion_object.set_prob(vision_object_array.bbox_3d(i));    //vision置信度未传出
-
-            iv::fusion::PointXYZ centroid;
-            iv::fusion::PointXYZ *centerpoint;
-//            Point pointIncamera, pointInworld;
-
-//            pointIncamera.x = camera_object_array.obj(i).outpointx();
-//            pointIncamera.y = camera_object_array.obj(i).outpointy();
-//            pointInworld = iv::fusion::Objectfusion::CameraToWorld(pointIncamera);
-            point_word = iv::fusion::Objectfusion::ComputeCameratToWord(vision_object_array.bbox_3d(i));
-
-            centroid.set_x(point_word.x);
-            centroid.set_y(point_word.z);
-            centerpoint=fusion_object.mutable_centroid();
-            centerpoint->CopyFrom(centroid);
-//            iv::fusion::VelXY vel_relative;
-//            iv::fusion::VelXY *velrelative;
-//            vel_relative.set_x(vision_object_array.bbox_3d(i));
-//            vel_relative.set_y(camera_object_array.obj(i).speedy());
-
-//            velrelative = fusion_object.mutable_vel_relative();
-//            velrelative->CopyFrom(vel_relative);
-            iv::fusion::Dimension dimension;
-            iv::fusion::Dimension *obj_dimension;
-            dimension.set_x(1);
-            dimension.set_y(1);
-            dimension.set_y(1);
-            obj_dimension = fusion_object.mutable_dimensions();
-            obj_dimension->CopyFrom(dimension);
-
-            iv::fusion::fusionobject *po = Ra_Ca_fusion_object_array.add_obj();
-            po->CopyFrom(fusion_object);
-        }else{
-
-            vector<float> dis;
-            cv::Point3f point_word;
-            point_word = iv::fusion::Objectfusion::ComputeCameratToWord(vision_object_array.bbox_3d(i));
-
-            cv::Point2f po1;
-            po1.x = point_word.x;
-            po1.y = point_word.z;
-            for(vector<int>::iterator d = fusionindex.begin(); d != fusionindex.end(); d++)
-            {
-                cv::Point2f po2;
-                po2.x = radar_object_array.obj(*d).x();
-                po2.y = radar_object_array.obj(*d).y();
-                dis.push_back(ComputeDis(po1,po2));
-            }
-            auto smallest = std::min_element(std::begin(dis), std::end(dis));
-            int index = std::distance(std::begin(dis), smallest);
-            dis.clear();
-
-            iv::fusion::fusionobject fusion_object;
-            fusion_object.set_id(vision_object_array.bbox_3d(i).class_id());
-            fusion_object.set_type_name(vision_object_array.bbox_3d(i).category());
-//            fusion_object.set_prob(vision_object_array.bbox_3d(i));   //vision置信度未传出
-
-            iv::fusion::PointXYZ centroid;
-            iv::fusion::PointXYZ *centerpoint;
-            centroid.set_x(radar_object_array.obj(fusionindex[index]).x());
-            centroid.set_y(radar_object_array.obj(fusionindex[index]).y());
-            centerpoint=fusion_object.mutable_centroid();
-            centerpoint->CopyFrom(centroid);
-//                 std::cout<<radar_object_array.obj(j).x()<<"~~~~~~~~~~"<<radar_object_array.obj(j).y()<<std::endl;
-
-            iv::fusion::Dimension dimension;
-            iv::fusion::Dimension *obj_dimension;
-            dimension.set_x(1);
-            dimension.set_y(1);
-            dimension.set_y(1);
-            obj_dimension = fusion_object.mutable_dimensions();
-            obj_dimension->CopyFrom(dimension);
-
-            iv::fusion::VelXY vel_relative;
-            iv::fusion::VelXY *velrelative;
-            vel_relative.set_x(radar_object_array.obj(fusionindex[index]).vx());
-            vel_relative.set_y(radar_object_array.obj(fusionindex[index]).vy());
-
-            velrelative = fusion_object.mutable_vel_relative();
-            velrelative->CopyFrom(vel_relative);
-
-            iv::fusion::fusionobject *pe = Ra_Ca_fusion_object_array.add_obj();
-            pe->CopyFrom(fusion_object);
-        }
-        fusionindex.clear();
-    }
-}
-
-
-

src/fusion/fusion_radar_camer/fusion.h

@@ -1,65 +0,0 @@
-#ifndef OBJECT_DISTANCE_H
-#define OBJECT_DISTANCE_H
-#include <eigen3/Eigen/Dense>
-#include "fusionobjectarray.pb.h"
-//#include "cameraobjectarray.pb.h"
-#include "obstacles.pb.h"
-#include "radarobjectarray.pb.h"
-#include "opencv2/opencv.hpp"
-#include "math.h"
-using namespace Eigen;
-
-struct Point{
-    float x;
-    float y;
-    float z;
-};
-
-//struct fusion_state{
-//    int s;
-//    float dis;
-//};
-
-namespace iv {
-
-namespace fusion {
-
-
-
-class Objectfusion{
-
-public:
-    Objectfusion(){};
-    ~Objectfusion(){};
-
-    // radar coordinate transport to camera coordinate
-    static void RadarToCamera(std::vector<cv::Point3f>& radarpoint, std::vector<cv::Point2f>& camerapoint);
-
-    static Point CameraToWorld(Point CameraInfo);
-
-    static void RadarPross(iv::radar::radarobjectarray& radar_object);
-
-    static float ComputelonDistance(double x, double y);
-
-    static float ComputeRadarSpeed(double vx, double vy);
-
-    static float ComputeDis(cv::Point2f po1,cv::Point2f po2);
-
-    static  void datafusion( iv::vision::ObstacleInfo& vision_object_array, iv::radar::radarobjectarray& radar_object_array, iv::fusion::fusionobjectarray& Ra_Ca_fusion_object_array);
-
-//    static fusion_state Computefusionprobabilities(const  iv::radar::radarobject& radar_object, const  iv::vision::cameraobject& camera_object );
-
-    static cv::Point3f ComputeCameratToWord(const iv::vision::Bbox3D& obs);
-
-    static int Computefusionprobabilities(const  iv::radar::radarobject& radar_object, const  iv::vision::Bbox3D& camera_object );
-
-    static int Computeradartobox(cv::Point2f radarincamera,Point top_left,Point bot_right);
-
-    };
-    }
-
-}
-
-
-
-#endif // OBJECT_DISTANCE_H

src/fusion/fusion_radar_camer/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/fusion_radar_camer/main.cpp

@@ -1,184 +0,0 @@
-
-#include <QCoreApplication>
-#include <QDateTime>
-#include <iostream>
-#include "modulecomm.h"
-#include "cameraobjectarray.pb.h"
-#include "objectarray.pb.h"
-#include "radarobjectarray.pb.h"
-#include "fusionobjectarray.pb.h"
-#include "rawpic.pb.h"
-#include "fusionobject.pb.h"
-#include <QThread>
-#include <QString>
-#include "opencv2/highgui.hpp"
-#include "opencv2/opencv.hpp"
-#include "imageBuffer.h"
-#include "fusion.h"
-#include <QMutex>
-#include "data_manager.h"
-
-#include "Tracking.h"
-
-#include <opencv2/opencv.hpp>
-#include <opencv2/core/ocl.hpp>
-
-using namespace iv;
-using namespace fusion;
-
-void * gfu = iv::modulecomm::RegisterSend("ra_ca_fusion",10000000,1);
-static QMutex gMutex;
-
-typedef  iv::radar::radarobjectarray RadarDataType;
-typedef  iv::lidar::objectarray LidarDataType;
-//typedef  iv::vision::cameraobjectarray CameraDataType;
-typedef  iv::vision::ObstacleInfo VisionDataType;
-typedef  std::chrono::system_clock::time_point TimeType;
-//typedef  std::function<void(RadarDataType&, LidarDataType&,VisionDataType&)> DataCallback;
-typedef  std::function<void(RadarDataType&,VisionDataType&)> RVCallback;
-void rv_callback(RadarDataType& radarobjv , VisionDataType& visionobjv);
-
-
-QTime gTime;
-using namespace std;
-int gntemp = 0;
-
-
-//iv::vision::cameraobjectarray* cameraobjs = new iv::vision::cameraobjectarray();
-//iv::radar::radarobjectarray* radarobjs = new iv::radar::radarobjectarray();
-iv::fusion::fusionobjectarray fusionobjvec;
-ConsumerProducerQueue<cv::Mat> *imageBuffer = new ConsumerProducerQueue<cv::Mat>(1, true);
-
-TrackerSettings settings;
-CTracker tracker(settings);
-void radarcamerafusion(iv::vision::ObstacleInfo& visionobjvec,iv::radar::radarobjectarray& radarobjvec);
-iv::DataManager data_manager;
-
-
-bool m_isTrackerInitialized = false;
-
-
-
-void Listenesrfront(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-//    std::cout<<"111111"<<std::endl;
-    iv::radar::radarobjectarray radarobj;
-    if(nSize<1)return;
-    if(false == radarobj.ParseFromArray(strdata,nSize))
-    {
-        std::cout<<"PecerptionShow Listenesrfront fail."<<std::endl;
-        return;
-    }
-    else{
-        //std::cout<<"srfront byte size:  "<<radarobjvec.ByteSize()<<std::endl;
-    }
-    gMutex.lock();
-//    for(int i = 0; i<radarobj.obj_size(); i++)
-//    {
-//        std::cout<<"x   :"<<radarobj.obj(i).x()<<"     y   :"<<radarobj.obj(i).y()<<std::endl;
-//    }
-    data_manager.SetRadar(radarobj);
-    gMutex.unlock();
-//    std::cout<<"-----------------------"<<std::endl;
-//    for(int j = 0; j<radarobj.obj_size(); j++)
-//    {
-//        if(!(radarobj.obj(j).bvalid()==true)) continue;
-//        std::cout<<"obj x: "<<radarobj.obj(j).x()<<"    obj y:"<<radarobj.obj(j).y()<<std::endl;
-//    }
-
-}
-
-void Listencamera(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-//    std::cout<<"22222"<<std::endl;
-    if(nSize<1)return;
-    iv::vision::ObstacleInfo visionobj;
-    if(false == visionobj.ParseFromArray(strdata, nSize))
-    {
-        //        std::cout<<"ccccccccccc Listencamera fail."<<std::endl;
-        return;
-    }
-    else{
-        //std::cout<<"camera byte size:  "<<cameraobjvec.ByteSize()<<std::endl;
-    }
-    gMutex.lock();
-    data_manager.SetCamera(visionobj);
-//    std::cout<<"camera byte size:  "<<visionobj.time()<<std::endl;
-//            for(int j = 0; j<visionobj.bbox_3d_size(); j++)
-//            {
-//                std::cout<<"obj x: "<<visionobj.bbox_3d(j).x()<<"    obj y:"<<visionobj.bbox_3d(j).z()<<std::endl;
-//            }
-    gMutex.unlock();
-}
-
-void rv_callback(RadarDataType& radarobjv ,VisionDataType& visionobjv)
-{
-    if((radarobjv.ByteSize() == 0) || (visionobjv.ByteSize() ==0) ) return;
-//    std::cout<<"radar time:  "<<radarobjv.mstime()<<std::endl;
-//    std::cout<<"camera  time:  "<<cameraobjv.mstime()<<std::endl;
-//     for(int i =0; i<radarobjv.obj_size(); i++)
-//     {
-//         std::cout<<"x   :"<<radarobjv.obj(i).x()<<"y   :"<<radarobjv.obj(i).y()<<std::endl;
-
-//     }
-    radarcamerafusion(visionobjv,radarobjv);
-//    gMutex.lock();
-//    cameraobjs->CopyFrom(cameraobjv);
-//    radarobjs->CopyFrom(radarobjv);
-//    gMutex.unlock();
-//    getpic();
-//    show_result();
-}
-
-void radarcamerafusion(iv::vision::ObstacleInfo& visionobjvec,iv::radar::radarobjectarray& radarobjvec)
-{
-   if(visionobjvec.bbox_3d_size() == 0 || radarobjvec.obj_size() ==0) return;
-    gMutex.lock();
-    Objectfusion::RadarPross(radarobjvec);
-    gMutex.unlock();
-    gMutex.lock();
-    std::cout<<"----start fusion----"<<std::endl;
-    Objectfusion::datafusion(visionobjvec,radarobjvec,fusionobjvec);
-    std::cout<<"----start track----"<<fusionobjvec.obj_size()<<std::endl;
-    if (!m_isTrackerInitialized)
-        {
-            m_isTrackerInitialized = InitTracker(tracker);
-            if (!m_isTrackerInitialized)
-            {
-                std::cerr << "Tracker initialize error!!!" << std::endl;
-            }
-        }
-        Tracking(fusionobjvec, tracker);
-        //--------------------------------------------  end tracking  ---------------------------------------------------
-
-
-        for(int j = 0; j<fusionobjvec.obj_size(); j++)
-        {
-            std::cout<<"obj x: "<<fusionobjvec.obj(j).centroid().x()<<"    obj y:"<<fusionobjvec.obj(j).centroid().y()<<std::endl;
-        }
-    gMutex.unlock();
- 
-    std::string out = fusionobjvec.SerializeAsString();
-    iv::modulecomm::ModuleSendMsg(gfu,out.data(),out.length());
-
-    qDebug("lenth is %d",out.length());
-
-}
-
-int main(int argc, char *argv[])
-{
-    QCoreApplication a(argc, argv);
-
-      tracker.setSettings(settings);
-      data_manager.SetRVCallback(rv_callback);
-      data_manager.Ready();
-
-      void *gpa;
-
-  //    gpa = iv::modulecomm::RegisterRecv("camera_detect",Listendetection);
-      gpa = iv::modulecomm::RegisterRecv("radar0",Listenesrfront);
-  //    gpa = iv::modulecomm::RegisterRecv("camera_output",Listencamera);
-      gpa = iv::modulecomm::RegisterRecv("vision_obstacles", Listencamera);
-
-      return a.exec();
-}

src/fusion/fusion_radar_camer/radar_camer_fusion.pro

@@ -1,90 +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
-
-QMAKE_LFLAGS += -no-pie
-
-# You can also make your code fail to compile if you use deprecated APIs.
-# In order to do so, uncomment the following line.
-# You can also select to disable deprecated APIs only up to a certain version of Qt.
-#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
-INCLUDEPATH += \
-Tracker
-
-SOURCES += \
-    fusion.cpp \
-        main.cpp \
-    ../../include/msgtype/radarobjectarray.pb.cc \
-    ../../include/msgtype/radarobject.pb.cc \
-    ../../include/msgtype/fusionobjectarray.pb.cc \
-    ../../include/msgtype/fusionobject.pb.cc \
-    ../../include/msgtype/cameraobjectarray.pb.cc \
-    ../../include/msgtype/cameraobject.pb.cc \
-    ../../include/msgtype/rawpic.pb.cc \ 
-    Tracker/HungarianAlg.cpp \
-    Tracker/Ctracker.cpp \
-    Tracker/Kalman.cpp \
-    Tracker/track.cpp \
-    ../../include/msgtype/areaarray.pb.cc \
-    ../../include/msgtype/obstacles.pb.cc
-
-
-# Default rules for deployment.
-qnx: target.path = /tmp/$${TARGET}/bin
-else: unix:!android: target.path = /opt/$${TARGET}/bin
-!isEmpty(target.path): INSTALLS += target
-
-HEADERS += \
-    ../../include/msgtype/radarobjectarray.pb.h \
-    ../../include/msgtype/radarobject.pb.h \
-    ../../include/msgtype/fusionobjectarray.pb.h \
-    ../../include/msgtype/fusionobject.pb.h \
-    ../../include/msgtype/cameraobjectarray.pb.h \
-    ../../include/msgtype/cameraobject.pb.h \
-    fusion.h \
-    imageBuffer.h \
-    ../../include/msgtype/rawpic.pb.h \
-    Tracker/defines.h \
-    Tracker/HungarianAlg.h \
-    Tracker/Ctracker.h \
-    Tracker/Kalman.h \
-    Tracker/ShortPathCalculator.h \
-    Tracker/track.h \
-    Tracker/VOTTracker.hpp \
-    ../../include/msgtype/obstacles.pb.h
-
-INCLUDEPATH += /usr/include/eigen3
-INCLUDEPATH += $$PWD/../../include/msgtype
-
-
-LIBS += -lprotobuf
-
-#INCLUDEPATH += \
-#$$PWD/../../common/xmlparam \
-#$$PWD/../../common/modulecomm \
-#$$PWD/../../common/ivlog \
-#$$PWD/../../common/ivfault \
-#$$PWD/../../common/Time_Manager_0714/
-#LIBS += \
-#$$PWD/../../common/xmlparam/libxmlparam.so \
-#$$PWD/../../common/modulecomm/libmodulecomm.so \
-#$$PWD/../../common/ivlog/libivlog.so \
-#$$PWD/../../common/ivfault/libivfault.so
-INCLUDEPATH += $$PWD/../../../include/
-
-LIBS += -L$$PWD/../../../bin -lxmlparam -lmodulecomm -livlog -livfault -ldatamanager
-
-#INCLUDEPATH += /usr/include/
-
-LIBS += /usr/lib/libopencv*.so
-
-#INCLUDEPATH += /opt/ros/kinetic/include/opencv-3.3.1-dev
-
-#LIBS += /opt/ros/kinetic/lib/x86_64-linux-gnu/libopencv*.so
-

src/fusion/radar_camer_fusion/.qmake.stash

@@ -1,24 +0,0 @@
-QMAKE_CXX.QT_COMPILER_STDCXX = 201402L
-QMAKE_CXX.QMAKE_GCC_MAJOR_VERSION = 7
-QMAKE_CXX.QMAKE_GCC_MINOR_VERSION = 5
-QMAKE_CXX.QMAKE_GCC_PATCH_VERSION = 0
-QMAKE_CXX.COMPILER_MACROS = \
-    QT_COMPILER_STDCXX \
-    QMAKE_GCC_MAJOR_VERSION \
-    QMAKE_GCC_MINOR_VERSION \
-    QMAKE_GCC_PATCH_VERSION
-QMAKE_CXX.INCDIRS = \
-    /usr/include/c++/7 \
-    /usr/include/x86_64-linux-gnu/c++/7 \
-    /usr/include/c++/7/backward \
-    /usr/lib/gcc/x86_64-linux-gnu/7/include \
-    /usr/local/include \
-    /usr/lib/gcc/x86_64-linux-gnu/7/include-fixed \
-    /usr/include/x86_64-linux-gnu \
-    /usr/include
-QMAKE_CXX.LIBDIRS = \
-    /usr/lib/gcc/x86_64-linux-gnu/7 \
-    /usr/lib/x86_64-linux-gnu \
-    /usr/lib \
-    /lib/x86_64-linux-gnu \
-    /lib

src/fusion/radar_camer_fusion/Tracker/Ctracker.cpp

@@ -1,275 +0,0 @@
-#include "Ctracker.h"
-///
-/// \brief CTracker::CTracker
-/// Tracker. Manage tracks. Create, remove, update.
-/// \param settings
-///
-CTracker::CTracker(TrackerSettings& settings)
-{
-    m_settings = settings;
-}
-///
-/// \brief CTracker::CTracker
-/// Tracker. Manage tracks. Create, remove, update.
-/// \param settings
-///
-CTracker::CTracker(const TrackerSettings& settings)
-    :
-      m_settings(settings),
-      m_nextTrackID(0)
-{
-    ShortPathCalculator* spcalc = nullptr;
-    SPSettings spSettings = { settings.m_distThres, 12 };
-    switch (m_settings.m_matchType)
-    {
-    case tracking::MatchHungrian:
-        spcalc = new SPHungrian(spSettings);
-        break;
-        //    case tracking::MatchBipart:
-        //        spcalc = new SPBipart(spSettings);
-        //        break;
-    }
-    assert(spcalc != nullptr);
-    m_SPCalculator = std::unique_ptr<ShortPathCalculator>(spcalc);
-}
-
-///
-/// \brief CTracker::~CTracker
-///
-CTracker::~CTracker(void)
-{
-}
-///
-/// \brief CTracker::setSettings
-/// Tracker. Manage tracks. Create, remove, update.
-/// \param settings
-///
-void CTracker::setSettings(TrackerSettings& settings)
-{
-    m_settings = settings;
-    m_nextTrackID = 0;
-    ShortPathCalculator* spcalc = nullptr;
-    SPSettings spSettings = { settings.m_distThres, 12 };
-    switch (m_settings.m_matchType)
-    {
-    case tracking::MatchHungrian:
-        spcalc = new SPHungrian(spSettings);
-        break;
-        //    case tracking::MatchBipart:
-        //        spcalc = new SPBipart(spSettings);
-        //        break;
-    }
-    assert(spcalc != nullptr);
-    m_SPCalculator = std::unique_ptr<ShortPathCalculator>(spcalc);
-}
-///
-/// \brief CTracker::Update
-/// \param regions
-/// \param currFrame
-/// \param fps
-///
-void CTracker::Update(
-        const regions_t& regions,
-        cv::UMat currFrame,
-        float fps
-        )
-{
-    UpdateTrackingState(regions, currFrame, fps);
-
-    currFrame.copyTo(m_prevFrame);
-}
-
-///
-/// \brief CTracker::UpdateTrackingState
-/// \param regions
-/// \param currFrame
-/// \param fps
-///
-void CTracker::UpdateTrackingState(
-        const regions_t& regions,
-        cv::UMat currFrame,
-        float fps
-        )
-{
-    const size_t N = m_tracks.size();	// Tracking objects
-    const size_t M = regions.size();	// Detections or regions
-
-    assignments_t assignment(N, -1); // Assignments regions -> tracks
-
-    if (!m_tracks.empty())
-    {
-        // 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);
-        track_t maxCost = 0;
-        CreateDistaceMatrix(regions, costMatrix, maxPossibleCost, maxCost, currFrame);
-
-        // Solving assignment problem (shortest paths)
-        m_SPCalculator->Solve(costMatrix, N, M, assignment, maxCost);//row->col(trackid->regionid)
-
-        // clean assignment from pairs with large distance
-        for (size_t i = 0; i < assignment.size(); i++)
-        {
-            if (assignment[i] != -1)
-            {
-                if (costMatrix[i + assignment[i] * N] > m_settings.m_distThres)
-                {
-                    assignment[i] = -1;
-                    m_tracks[i]->SkippedFrames()++;
-                }
-            }
-            else
-            {
-                // If track have no assigned detect, then increment skipped frames counter.
-                m_tracks[i]->SkippedFrames()++;
-            }
-            m_tracks[i]->m_regionID = assignment[i];
-
-        }
-
-        // 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);
-                assignment.erase(assignment.begin() + i);
-            }
-            else
-            {
-                ++i;
-            }
-        }
-    }
-
-    // Search for unassigned detects and start new tracks for them.
-    for (size_t i = 0; i < regions.size(); ++i)
-    {
-        if (find(assignment.begin(), assignment.end(), i) == assignment.end())
-        {
-            m_tracks.push_back(std::make_unique<CTrack>(regions[i],
-                                                        m_settings.m_kalmanType,
-                                                        m_settings.m_dt,
-                                                        m_settings.m_accelNoiseMag,
-                                                        m_settings.m_useAcceleration,
-                                                        m_nextTrackID++%500,
-                                                        i,
-                                                        m_settings.m_filterGoal == tracking::FilterRect,
-                                                        m_settings.m_lostTrackType));
-        }
-    }
-
-    // Update Kalman Filters state
-    const ptrdiff_t stop_i = static_cast<ptrdiff_t>(assignment.size());
-#pragma omp parallel for
-    for (ptrdiff_t i = 0; i < stop_i; ++i)
-    {
-        // 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]->SkippedFrames() = 0;
-            m_tracks[i]->Update(
-                        regions[assignment[i]], true,
-                    m_settings.m_maxTraceLength,
-                    m_prevFrame, currFrame,
-                    m_settings.m_useAbandonedDetection ? cvRound(m_settings.m_minStaticTime * fps) : 0);
-        }
-        else				     // if not continue using predictions
-        {
-            m_tracks[i]->Update(CRegion(), false, m_settings.m_maxTraceLength, m_prevFrame, currFrame, 0);
-        }
-    }
-}
-
-///
-/// \brief CTracker::CreateDistaceMatrix
-/// \param regions
-/// \param costMatrix
-/// \param maxPossibleCost
-/// \param maxCost
-///
-void CTracker::CreateDistaceMatrix(const regions_t& regions, distMatrix_t& costMatrix, track_t maxPossibleCost, track_t& maxCost, cv::UMat currFrame)
-{
-    const size_t N = m_tracks.size();	// Tracking objects
-    maxCost = 0;
-
-    for (size_t i = 0; i < N; ++i)
-    {
-        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);
-
-        // Calc distance between track and regions
-        for (size_t j = 0; j < regions.size(); ++j)
-        {
-            const auto& reg = regions[j];
-
-            auto dist = maxPossibleCost;
-            if (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
-                    track_t ellipseDist = track->IsInsideArea(reg.m_rrect.center, predictedArea);
-                    if (ellipseDist > 1)
-                        dist += m_settings.m_distType[ind];
-                    else
-                        dist += ellipseDist * m_settings.m_distType[ind];
-#else
-                    dist += m_settings.m_distType[ind] * track->CalcDistCenter(reg);
-#endif
-                }
-                ++ind;
-
-                if (m_settings.m_distType[ind] > 0.0f && ind == tracking::DistRects)
-                {
-#if 1
-                    track_t ellipseDist = track->IsInsideArea(reg.m_rrect.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->CalcDistRect(reg);
-#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);
-                ++ind;
-                assert(ind == tracking::DistsCount);
-            }
-            costMatrix[i + j * N] = dist;
-            if (dist > maxCost)
-                maxCost = dist;
-        }
-    }
-}

src/fusion/radar_camer_fusion/Tracker/Ctracker.h

@@ -1,257 +0,0 @@
-#pragma once
-#include <iostream>
-#include <vector>
-#include <memory>
-#include <array>
-#include <deque>
-#include <numeric>
-#include <map>
-#include <set>
-
-#include "defines.h"
-#include "track.h"
-#include "ShortPathCalculator.h"
-
-// ----------------------------------------------------------------------
-
-///
-/// \brief The TrackerSettings struct
-///
-struct TrackerSettings
-{
-    tracking::KalmanType m_kalmanType = tracking::KalmanLinear;
-    tracking::FilterGoal m_filterGoal = tracking::FilterCenter;
-    tracking::LostTrackType m_lostTrackType = tracking::TrackKCF;
-    tracking::MatchType m_matchType = tracking::MatchHungrian;
-
-	std::array<track_t, tracking::DistsCount> m_distType;
-
-    ///
-    /// \brief m_dt
-    /// Time step for Kalman
-    ///
-    track_t m_dt = 1.0f;
-
-    ///
-    /// \brief m_accelNoiseMag
-    /// Noise magnitude for Kalman
-    ///
-    track_t m_accelNoiseMag = 0.1f;
-
-	///
-	/// \brief m_useAcceleration
-	/// Constant velocity or constant acceleration motion model
-	///
-	bool m_useAcceleration = false;
-
-    ///
-    /// \brief m_distThres
-    /// Distance threshold for Assignment problem: from 0 to 1
-    ///
-    track_t m_distThres = 0.8f;
-
-    ///
-    /// \brief m_minAreaRadius
-    /// Minimal area radius in pixels for objects centers
-    ///
-    track_t m_minAreaRadiusPix = 20.f;
-
-	///
-	/// \brief m_minAreaRadius
-	/// Minimal area radius in ration for object size.. Used if m_minAreaRadiusPix < 0
-	///
-	track_t m_minAreaRadiusK = 0.5f;
-
-    ///
-    /// \brief m_maximumAllowedSkippedFrames
-    /// If the object don't assignment more than this frames then it will be removed
-    ///
-    size_t m_maximumAllowedSkippedFrames = 25;
-
-    ///
-    /// \brief m_maxTraceLength
-    /// The maximum trajectory length
-    ///
-    size_t m_maxTraceLength = 50;
-
-    ///
-    /// \brief m_useAbandonedDetection
-    /// Detection abandoned objects
-    ///
-    bool m_useAbandonedDetection = false;
-
-    ///
-    /// \brief m_minStaticTime
-    /// After this time (in seconds) the object is considered abandoned
-    ///
-    int m_minStaticTime = 5;
-    ///
-    /// \brief m_maxStaticTime
-    /// After this time (in seconds) the abandoned object will be removed
-    ///
-    int m_maxStaticTime = 25;
-
-	///
-	/// \brief m_nearTypes
-	/// Object types that can be matched while tracking
-	///
-	std::map<std::string, std::set<std::string>> m_nearTypes;
-
-	///
-	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;
-
-		assert(CheckDistance());
-	}
-
-	///
-	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));
-		return std::fabs(sum - 1.0f) <= std::numeric_limits<track_t>::epsilon() * maxOne;
-	}
-
-	///
-	bool SetDistances(std::array<track_t, tracking::DistsCount> distType)
-	{
-		bool res = true;
-		auto oldDists = m_distType;
-		m_distType = distType;
-		if (!CheckDistance())
-		{
-			m_distType = oldDists;
-			res = false;
-		}
-		return res;
-	}
-
-	///
-	bool SetDistance(tracking::DistType distType)
-	{
-		std::fill(m_distType.begin(), m_distType.end(), 0.0f);
-		m_distType[distType] = 1.f;
-		return true;
-	}
-
-	///
-	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_nearTypes.find(type1);
-			if (it == std::end(m_nearTypes))
-			{
-				m_nearTypes[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_nearTypes.find(type1);
-			if (it != std::end(m_nearTypes))
-			{
-				res = it->second.find(type2) != std::end(it->second);
-			}
-		}
-		return res;
-	}
-};
-
-///
-/// \brief The CTracker class
-///
-class CTracker
-{
-public:
-    CTracker(TrackerSettings& settings);
-    CTracker(const TrackerSettings& settings);
-    CTracker(const CTracker&) = delete;
-    CTracker(CTracker&&) = delete;
-	CTracker& operator=(const CTracker&) = delete;
-	CTracker& operator=(CTracker&&) = delete;
-	
-	~CTracker(void);
-    void setSettings(TrackerSettings& settings);
-    void Update(const regions_t& regions, cv::UMat currFrame, float fps);
-
-    ///
-    /// \brief CanGrayFrameToTrack
-    /// \return
-    ///
-    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);*/
-        return !needColor;
-    }
-
-	///
-	/// \brief CanColorFrameToTrack
-	/// \return
-	///
-	bool CanColorFrameToTrack() const
-	{
-		return true;
-	}
-
-    ///
-    /// \brief GetTracksCount
-    /// \return
-    ///
-	size_t GetTracksCount() const
-	{
-		return m_tracks.size();
-	}
-    ///
-    /// \brief GetTracks
-    /// \return
-    ///
-	std::vector<TrackingObject> GetTracks() const
-	{
-		std::vector<TrackingObject> tracks;
-		if (!m_tracks.empty())
-		{
-			tracks.reserve(m_tracks.size());
-			for (const auto& track : m_tracks)
-			{
-                tracks.push_back(track->ConstructObject());
-			}
-		}
-		return tracks;
-	}
-
-private:
-    TrackerSettings m_settings;
-
-	tracks_t m_tracks;
-
-    size_t m_nextTrackID;
-
-    cv::UMat m_prevFrame;
-
-    std::unique_ptr<ShortPathCalculator> m_SPCalculator;
-
-    void CreateDistaceMatrix(const regions_t& regions, distMatrix_t& costMatrix, track_t maxPossibleCost, track_t& maxCost, cv::UMat currFrame);
-    void UpdateTrackingState(const regions_t& regions, cv::UMat currFrame, float fps);
-};

src/fusion/radar_camer_fusion/Tracker/HungarianAlg.cpp

@@ -1,723 +0,0 @@
-#include "HungarianAlg.h"
-#include <limits>
-
-// --------------------------------------------------------------------------
-//
-// --------------------------------------------------------------------------
-AssignmentProblemSolver::AssignmentProblemSolver()
-{
-}
-
-// --------------------------------------------------------------------------
-//
-// --------------------------------------------------------------------------
-AssignmentProblemSolver::~AssignmentProblemSolver()
-{
-}
-
-// --------------------------------------------------------------------------
-//
-// --------------------------------------------------------------------------
-track_t AssignmentProblemSolver::Solve(
-	const distMatrix_t& distMatrixIn,
-	size_t nOfRows,
-	size_t nOfColumns,
-	std::vector<int>& assignment,
-	TMethod Method
-	)
-{
-	assignment.resize(nOfRows, -1);
-
-	track_t cost = 0;
-
-	switch (Method)
-	{
-	case optimal:
-		assignmentoptimal(assignment, cost, distMatrixIn, nOfRows, nOfColumns);
-		break;
-
-	case many_forbidden_assignments:
-		assignmentsuboptimal1(assignment, cost, distMatrixIn, nOfRows, nOfColumns);
-		break;
-
-	case without_forbidden_assignments:
-		assignmentsuboptimal2(assignment, cost, distMatrixIn, nOfRows, nOfColumns);
-		break;
-	}
-
-	return cost;
-}
-// --------------------------------------------------------------------------
-// Computes the optimal assignment (minimum overall costs) using Munkres algorithm.
-// --------------------------------------------------------------------------
-void AssignmentProblemSolver::assignmentoptimal(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns)
-{
-	// Generate distance cv::Matrix 
-	// and check cv::Matrix elements positiveness :)
-
-	// Total elements number
-	size_t nOfElements = nOfRows * nOfColumns;
-	// Memory allocation
-	track_t* distMatrix = (track_t *)malloc(nOfElements * sizeof(track_t));
-
-    if (distMatrix == nullptr)
-    {
-        return;
-    }
-
-	// Pointer to last element
-	track_t* distMatrixEnd = distMatrix + nOfElements;
-
-	for (size_t row = 0; row < nOfElements; row++)
-	{
-        track_t value = distMatrixIn[row];
-		assert(value >= 0);
-		distMatrix[row] = value;
-	}
-
-	// Memory allocation
-	bool* coveredColumns = (bool*)calloc(nOfColumns, sizeof(bool));
-	bool* coveredRows = (bool*)calloc(nOfRows, sizeof(bool));
-	bool* starMatrix = (bool*)calloc(nOfElements, sizeof(bool));
-	bool* primeMatrix = (bool*)calloc(nOfElements, sizeof(bool));
-	bool* newStarMatrix = (bool*)calloc(nOfElements, sizeof(bool)); /* used in step4 */
-
-	/* preliminary steps */
-	if (nOfRows <= nOfColumns)
-	{
-		for (size_t row = 0; row < nOfRows; row++)
-		{
-			/* find the smallest element in the row */
-            track_t* distMatrixTemp = distMatrix + row;
-			track_t  minValue = *distMatrixTemp;
-			distMatrixTemp += nOfRows;
-			while (distMatrixTemp < distMatrixEnd)
-			{
-				track_t value = *distMatrixTemp;
-				if (value < minValue)
-				{
-					minValue = value;
-				}
-				distMatrixTemp += nOfRows;
-			}
-			/* subtract the smallest element from each element of the row */
-			distMatrixTemp = distMatrix + row;
-			while (distMatrixTemp < distMatrixEnd)
-			{
-				*distMatrixTemp -= minValue;
-				distMatrixTemp += nOfRows;
-			}
-		}
-		/* Steps 1 and 2a */
-		for (size_t row = 0; row < nOfRows; row++)
-		{
-			for (size_t col = 0; col < nOfColumns; col++)
-			{
-				if (distMatrix[row + nOfRows*col] == 0)
-				{
-					if (!coveredColumns[col])
-					{
-						starMatrix[row + nOfRows * col] = true;
-						coveredColumns[col] = true;
-						break;
-					}
-				}
-			}
-		}
-	}
-	else /* if(nOfRows > nOfColumns) */
-	{
-		for (size_t col = 0; col < nOfColumns; col++)
-		{
-			/* find the smallest element in the column */
-			track_t* distMatrixTemp = distMatrix + nOfRows*col;
-			track_t* columnEnd = distMatrixTemp + nOfRows;
-			track_t  minValue = *distMatrixTemp++;
-			while (distMatrixTemp < columnEnd)
-			{
-				track_t value = *distMatrixTemp++;
-				if (value < minValue)
-				{
-					minValue = value;
-				}
-			}
-			/* subtract the smallest element from each element of the column */
-			distMatrixTemp = distMatrix + nOfRows*col;
-			while (distMatrixTemp < columnEnd)
-			{
-				*distMatrixTemp++ -= minValue;
-			}
-		}
-		/* Steps 1 and 2a */
-		for (size_t col = 0; col < nOfColumns; col++)
-		{
-			for (size_t row = 0; row < nOfRows; row++)
-			{
-				if (distMatrix[row + nOfRows*col] == 0)
-				{
-					if (!coveredRows[row])
-					{
-						starMatrix[row + nOfRows*col] = true;
-						coveredColumns[col] = true;
-						coveredRows[row] = true;
-						break;
-					}
-				}
-			}
-		}
-
-		for (size_t row = 0; row < nOfRows; row++)
-		{
-			coveredRows[row] = false;
-		}
-	}
-	/* move to step 2b */
-	step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, (nOfRows <= nOfColumns) ? nOfRows : nOfColumns);
-	/* compute cost and remove invalid assignments */
-	computeassignmentcost(assignment, cost, distMatrixIn, nOfRows);
-	/* free allocated memory */
-	free(distMatrix);
-	free(coveredColumns);
-	free(coveredRows);
-	free(starMatrix);
-	free(primeMatrix);
-	free(newStarMatrix);
-	return;
-}
-// --------------------------------------------------------------------------
-//
-// --------------------------------------------------------------------------
-void AssignmentProblemSolver::buildassignmentvector(assignments_t& assignment, bool *starMatrix, size_t nOfRows, size_t nOfColumns)
-{
-    for (size_t row = 0; row < nOfRows; row++)
-	{
-        for (size_t col = 0; col < nOfColumns; col++)
-		{
-			if (starMatrix[row + nOfRows * col])
-			{
-				assignment[row] = static_cast<int>(col);
-				break;
-			}
-		}
-	}
-}
-// --------------------------------------------------------------------------
-//
-// --------------------------------------------------------------------------
-void AssignmentProblemSolver::computeassignmentcost(const assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows)
-{
-	for (size_t row = 0; row < nOfRows; row++)
-	{
-		const int col = assignment[row];
-		if (col >= 0)
-		{
-			cost += distMatrixIn[row + nOfRows * col];
-		}
-	}
-}
-
-// --------------------------------------------------------------------------
-//
-// --------------------------------------------------------------------------
-void AssignmentProblemSolver::step2a(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim)
-{
-    bool *starMatrixTemp, *columnEnd;
-    /* cover every column containing a starred zero */
-    for (size_t col = 0; col < nOfColumns; col++)
-    {
-        starMatrixTemp = starMatrix + nOfRows * col;
-        columnEnd = starMatrixTemp + nOfRows;
-        while (starMatrixTemp < columnEnd)
-        {
-            if (*starMatrixTemp++)
-            {
-                coveredColumns[col] = true;
-                break;
-            }
-        }
-    }
-    /* move to step 3 */
-	step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
-}
-
-// --------------------------------------------------------------------------
-//
-// --------------------------------------------------------------------------
-void AssignmentProblemSolver::step2b(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim)
-{
-	/* count covered columns */
-    size_t nOfCoveredColumns = 0;
-    for (size_t col = 0; col < nOfColumns; col++)
-	{
-		if (coveredColumns[col])
-		{
-			nOfCoveredColumns++;
-		}
-	}
-	if (nOfCoveredColumns == minDim)
-	{
-		/* algorithm finished */
-		buildassignmentvector(assignment, starMatrix, nOfRows, nOfColumns);
-	}
-	else
-	{
-		/* move to step 3 */
-		step3_5(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
-	}
-}
-
-// --------------------------------------------------------------------------
-//
-// --------------------------------------------------------------------------
-void AssignmentProblemSolver::step3_5(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim)
-{
-	for (;;)
-	{
-		/* step 3 */
-		bool zerosFound = true;
-		while (zerosFound)
-		{
-			zerosFound = false;
-			for (size_t col = 0; col < nOfColumns; col++)
-			{
-				if (!coveredColumns[col])
-				{
-					for (size_t row = 0; row < nOfRows; row++)
-					{
-						if ((!coveredRows[row]) && (distMatrix[row + nOfRows*col] == 0))
-						{
-							/* prime zero */
-							primeMatrix[row + nOfRows*col] = true;
-							/* find starred zero in current row */
-							size_t starCol = 0;
-							for (; starCol < nOfColumns; starCol++)
-							{
-								if (starMatrix[row + nOfRows * starCol])
-								{
-									break;
-								}
-							}
-							if (starCol == nOfColumns) /* no starred zero found */
-							{
-								/* move to step 4 */
-								step4(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim, row, col);
-								return;
-							}
-							else
-							{
-								coveredRows[row] = true;
-								coveredColumns[starCol] = false;
-								zerosFound = true;
-								break;
-							}
-						}
-					}
-				}
-			}
-		}
-		/* step 5 */
-        track_t h = std::numeric_limits<track_t>::max();
-		for (size_t row = 0; row < nOfRows; row++)
-		{
-			if (!coveredRows[row])
-			{
-				for (size_t col = 0; col < nOfColumns; col++)
-				{
-					if (!coveredColumns[col])
-					{
-                        const track_t value = distMatrix[row + nOfRows*col];
-						if (value < h)
-						{
-							h = value;
-						}
-					}
-				}
-			}
-		}
-		/* add h to each covered row */
-		for (size_t row = 0; row < nOfRows; row++)
-		{
-			if (coveredRows[row])
-			{
-				for (size_t col = 0; col < nOfColumns; col++)
-				{
-					distMatrix[row + nOfRows*col] += h;
-				}
-			}
-		}
-		/* subtract h from each uncovered column */
-		for (size_t col = 0; col < nOfColumns; col++)
-		{
-			if (!coveredColumns[col])
-			{
-				for (size_t row = 0; row < nOfRows; row++)
-				{
-					distMatrix[row + nOfRows*col] -= h;
-				}
-			}
-		}
-	}
-}
-
-// --------------------------------------------------------------------------
-//
-// --------------------------------------------------------------------------
-void AssignmentProblemSolver::step4(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim, size_t row, size_t col)
-{
-	const size_t nOfElements = nOfRows * nOfColumns;
-	/* generate temporary copy of starMatrix */
-	for (size_t n = 0; n < nOfElements; n++)
-	{
-		newStarMatrix[n] = starMatrix[n];
-	}
-	/* star current zero */
-	newStarMatrix[row + nOfRows*col] = true;
-	/* find starred zero in current column */
-	size_t starCol = col;
-	size_t starRow = 0;
-	for (; starRow < nOfRows; starRow++)
-	{
-		if (starMatrix[starRow + nOfRows * starCol])
-		{
-			break;
-		}
-	}
-	while (starRow < nOfRows)
-	{
-		/* unstar the starred zero */
-		newStarMatrix[starRow + nOfRows*starCol] = false;
-		/* find primed zero in current row */
-		size_t primeRow = starRow;
-		size_t primeCol = 0;
-		for (; primeCol < nOfColumns; primeCol++)
-		{
-			if (primeMatrix[primeRow + nOfRows * primeCol])
-			{
-				break;
-			}
-		}
-		/* star the primed zero */
-		newStarMatrix[primeRow + nOfRows*primeCol] = true;
-		/* find starred zero in current column */
-		starCol = primeCol;
-		for (starRow = 0; starRow < nOfRows; starRow++)
-		{
-			if (starMatrix[starRow + nOfRows * starCol])
-			{
-				break;
-			}
-		}
-	}
-	/* use temporary copy as new starMatrix */
-	/* delete all primes, uncover all rows */
-    for (size_t n = 0; n < nOfElements; n++)
-	{
-		primeMatrix[n] = false;
-		starMatrix[n] = newStarMatrix[n];
-	}
-    for (size_t n = 0; n < nOfRows; n++)
-	{
-		coveredRows[n] = false;
-	}
-	/* move to step 2a */
-	step2a(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
-}
-
-// --------------------------------------------------------------------------
-// Computes a suboptimal solution. Good for cases without forbidden assignments.
-// --------------------------------------------------------------------------
-void AssignmentProblemSolver::assignmentsuboptimal2(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns)
-{
-	/* make working copy of distance Matrix */
-	const size_t nOfElements = nOfRows * nOfColumns;
-	track_t* distMatrix = (track_t*)malloc(nOfElements * sizeof(track_t));
-	for (size_t n = 0; n < nOfElements; n++)
-	{
-		distMatrix[n] = distMatrixIn[n];
-	}
-
-	/* recursively search for the minimum element and do the assignment */
-	for (;;)
-	{
-		/* find minimum distance observation-to-track pair */
-		track_t minValue = std::numeric_limits<track_t>::max();
-		size_t tmpRow = 0;
-		size_t tmpCol = 0;
-		for (size_t row = 0; row < nOfRows; row++)
-		{
-			for (size_t col = 0; col < nOfColumns; col++)
-			{
-				const track_t value = distMatrix[row + nOfRows*col];
-				if (value != std::numeric_limits<track_t>::max() && (value < minValue))
-				{
-					minValue = value;
-					tmpRow = row;
-					tmpCol = col;
-				}
-			}
-		}
-
-		if (minValue != std::numeric_limits<track_t>::max())
-		{
-			assignment[tmpRow] = static_cast<int>(tmpCol);
-			cost += minValue;
-			for (size_t n = 0; n < nOfRows; n++)
-			{
-				distMatrix[n + nOfRows*tmpCol] = std::numeric_limits<track_t>::max();
-			}
-			for (size_t n = 0; n < nOfColumns; n++)
-			{
-				distMatrix[tmpRow + nOfRows*n] = std::numeric_limits<track_t>::max();
-			}
-		}
-		else
-		{
-			break;
-		}
-	}
-
-	free(distMatrix);
-}
-// --------------------------------------------------------------------------
-// Computes a suboptimal solution. Good for cases with many forbidden assignments.
-// --------------------------------------------------------------------------
-void AssignmentProblemSolver::assignmentsuboptimal1(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns)
-{
-	/* make working copy of distance Matrix */
-	const size_t nOfElements = nOfRows * nOfColumns;
-	track_t* distMatrix = (track_t *)malloc(nOfElements * sizeof(track_t));
-    for (size_t n = 0; n < nOfElements; n++)
-	{
-		distMatrix[n] = distMatrixIn[n];
-	}
-
-	/* allocate memory */
-	int* nOfValidObservations = (int *)calloc(nOfRows, sizeof(int));
-	int* nOfValidTracks = (int *)calloc(nOfColumns, sizeof(int));
-
-	/* compute number of validations */
-	bool infiniteValueFound = false;
-	bool finiteValueFound = false;
-	for (size_t row = 0; row < nOfRows; row++)
-	{
-		for (size_t col = 0; col < nOfColumns; col++)
-		{
-			if (distMatrix[row + nOfRows*col] != std::numeric_limits<track_t>::max())
-			{
-				nOfValidTracks[col] += 1;
-				nOfValidObservations[row] += 1;
-				finiteValueFound = true;
-			}
-			else
-			{
-				infiniteValueFound = true;
-			}
-		}
-	}
-
-	if (infiniteValueFound)
-	{
-		if (!finiteValueFound)
-		{
-            /* free allocated memory */
-            free(nOfValidObservations);
-            free(nOfValidTracks);
-            free(distMatrix);
-
-			return;
-		}
-		bool repeatSteps = true;
-
-		while (repeatSteps)
-		{
-			repeatSteps = false;
-
-			/* step 1: reject assignments of multiply validated tracks to singly validated observations		 */
-			for (size_t col = 0; col < nOfColumns; col++)
-			{
-				bool singleValidationFound = false;
-				for (size_t row = 0; row < nOfRows; row++)
-				{
-					if (distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max() && (nOfValidObservations[row] == 1))
-					{
-						singleValidationFound = true;
-						break;
-					}
-				}
-				if (singleValidationFound)
-				{
-					for (size_t nestedRow = 0; nestedRow < nOfRows; nestedRow++)
-						if ((nOfValidObservations[nestedRow] > 1) && distMatrix[nestedRow + nOfRows * col] != std::numeric_limits<track_t>::max())
-						{
-							distMatrix[nestedRow + nOfRows * col] = std::numeric_limits<track_t>::max();
-							nOfValidObservations[nestedRow] -= 1;
-							nOfValidTracks[col] -= 1;
-							repeatSteps = true;
-						}
-				}
-			}
-
-			/* step 2: reject assignments of multiply validated observations to singly validated tracks */
-			if (nOfColumns > 1)
-			{
-				for (size_t row = 0; row < nOfRows; row++)
-				{
-					bool singleValidationFound = false;
-                    for (size_t col = 0; col < nOfColumns; col++)
-					{
-						if (distMatrix[row + nOfRows*col] != std::numeric_limits<track_t>::max() && (nOfValidTracks[col] == 1))
-						{
-							singleValidationFound = true;
-							break;
-						}
-					}
-
-					if (singleValidationFound)
-					{
-						for (size_t col = 0; col < nOfColumns; col++)
-						{
-							if ((nOfValidTracks[col] > 1) && distMatrix[row + nOfRows*col] != std::numeric_limits<track_t>::max())
-							{
-								distMatrix[row + nOfRows*col] = std::numeric_limits<track_t>::max();
-								nOfValidObservations[row] -= 1;
-								nOfValidTracks[col] -= 1;
-								repeatSteps = true;
-							}
-						}
-					}
-				}
-			}
-		} /* while(repeatSteps) */
-
-		/* for each multiply validated track that validates only with singly validated  */
-		/* observations, choose the observation with minimum distance */
-        for (size_t row = 0; row < nOfRows; row++)
-		{
-			if (nOfValidObservations[row] > 1)
-			{
-				bool allSinglyValidated = true;
-				track_t minValue = std::numeric_limits<track_t>::max();
-				size_t tmpCol = 0;
-                for (size_t col = 0; col < nOfColumns; col++)
-				{
-					const track_t value = distMatrix[row + nOfRows*col];
-					if (value != std::numeric_limits<track_t>::max())
-					{
-						if (nOfValidTracks[col] > 1)
-						{
-							allSinglyValidated = false;
-							break;
-						}
-						else if ((nOfValidTracks[col] == 1) && (value < minValue))
-						{
-							tmpCol = col;
-							minValue = value;
-						}
-					}
-				}
-
-				if (allSinglyValidated)
-				{
-					assignment[row] = static_cast<int>(tmpCol);
-					cost += minValue;
-					for (size_t n = 0; n < nOfRows; n++)
-					{
-						distMatrix[n + nOfRows*tmpCol] = std::numeric_limits<track_t>::max();
-					}
-					for (size_t n = 0; n < nOfColumns; n++)
-					{
-						distMatrix[row + nOfRows*n] = std::numeric_limits<track_t>::max();
-					}
-				}
-			}
-		}
-
-		// for each multiply validated observation that validates only with singly validated  track, choose the track with minimum distance
-        for (size_t col = 0; col < nOfColumns; col++)
-		{
-			if (nOfValidTracks[col] > 1)
-			{
-				bool allSinglyValidated = true;
-				track_t minValue = std::numeric_limits<track_t>::max();
-				size_t tmpRow = 0;
-				for (size_t row = 0; row < nOfRows; row++)
-				{
-					const track_t value = distMatrix[row + nOfRows*col];
-					if (value != std::numeric_limits<track_t>::max())
-					{
-						if (nOfValidObservations[row] > 1)
-						{
-							allSinglyValidated = false;
-							break;
-						}
-						else if ((nOfValidObservations[row] == 1) && (value < minValue))
-						{
-							tmpRow = row;
-							minValue = value;
-						}
-					}
-				}
-
-				if (allSinglyValidated)
-				{
-					assignment[tmpRow] = static_cast<int>(col);
-					cost += minValue;
-					for (size_t n = 0; n < nOfRows; n++)
-					{
-						distMatrix[n + nOfRows*col] = std::numeric_limits<track_t>::max();
-					}
-					for (size_t n = 0; n < nOfColumns; n++)
-					{
-						distMatrix[tmpRow + nOfRows*n] = std::numeric_limits<track_t>::max();
-					}
-				}
-			}
-		}
-	} /* if(infiniteValueFound) */
-
-
-	/* now, recursively search for the minimum element and do the assignment */
-	for (;;)
-	{
-		/* find minimum distance observation-to-track pair */
-		track_t minValue = std::numeric_limits<track_t>::max();
-		size_t tmpRow = 0;
-		size_t tmpCol = 0;
-		for (size_t row = 0; row < nOfRows; row++)
-		{
-			for (size_t col = 0; col < nOfColumns; col++)
-			{
-				const track_t value = distMatrix[row + nOfRows*col];
-				if (value != std::numeric_limits<track_t>::max() && (value < minValue))
-				{
-					minValue = value;
-					tmpRow = row;
-					tmpCol = col;
-				}
-			}
-		}
-
-		if (minValue != std::numeric_limits<track_t>::max())
-		{
-			assignment[tmpRow] = static_cast<int>(tmpCol);
-			cost += minValue;
-			for (size_t n = 0; n < nOfRows; n++)
-			{
-				distMatrix[n + nOfRows*tmpCol] = std::numeric_limits<track_t>::max();
-			}
-			for (size_t n = 0; n < nOfColumns; n++)
-			{
-				distMatrix[tmpRow + nOfRows*n] = std::numeric_limits<track_t>::max();
-			}
-		}
-		else
-		{
-			break;
-		}
-	}
-
-	/* free allocated memory */
-	free(nOfValidObservations);
-	free(nOfValidTracks);
-	free(distMatrix);
-}

src/fusion/radar_camer_fusion/Tracker/HungarianAlg.h

@@ -1,39 +0,0 @@
-#include <vector>
-#include <iostream>
-#include <limits>
-#include <time.h>
-#include "defines.h"
-// http://community.topcoder.com/tc?module=Static&d1=tutorials&d2=hungarianAlgorithm
-
-///
-/// \brief The AssignmentProblemSolver class
-///
-class AssignmentProblemSolver
-{
-private:
-	// Computes the optimal assignment (minimum overall costs) using Munkres algorithm.
-	void assignmentoptimal(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns);
-	void buildassignmentvector(assignments_t& assignment, bool *starMatrix, size_t nOfRows, size_t nOfColumns);
-	void computeassignmentcost(const assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows);
-	void step2a(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);
-	void step2b(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);
-	void step3_5(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);
-	void step4(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim, size_t row, size_t col);
-
-	// Computes a suboptimal solution. Good for cases with many forbidden assignments.
-	void assignmentsuboptimal1(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns);
-	// Computes a suboptimal solution. Good for cases with many forbidden assignments.
-	void assignmentsuboptimal2(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns);
-
-public:
-	enum TMethod
-	{
-		optimal,
-		many_forbidden_assignments,
-		without_forbidden_assignments
-	};
-
-	AssignmentProblemSolver();
-	~AssignmentProblemSolver();
-	track_t Solve(const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns, assignments_t& assignment, TMethod Method = optimal);
-};

src/fusion/radar_camer_fusion/Tracker/Kalman.cpp

@@ -1,921 +0,0 @@
-#include "Kalman.h"
-#include <iostream>
-#include <vector>
-
-//---------------------------------------------------------------------------
-TKalmanFilter::TKalmanFilter(
-        tracking::KalmanType type,
-	    bool useAcceleration,
-        track_t deltaTime, // time increment (lower values makes target more "massive")
-        track_t accelNoiseMag
-        )
-    :
-      m_type(type),
-      m_initialized(false),
-      m_deltaTime(deltaTime),
-      m_deltaTimeMin(deltaTime),
-      m_deltaTimeMax(2 * deltaTime),
-      m_lastDist(0),
-      m_accelNoiseMag(accelNoiseMag),
-	  m_useAcceleration(useAcceleration)
-{
-    m_deltaStep = (m_deltaTimeMax - m_deltaTimeMin) / m_deltaStepsCount;
-}
-
-//---------------------------------------------------------------------------
-void TKalmanFilter::CreateLinear(Point_t xy0, Point_t 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.
-    // Process noise. (standard deviation of acceleration: m/s^2)
-    // shows, woh much target can accelerate.
-
-    // 4 state variables, 2 measurements
-    m_linearKalman.init(4, 2, 0, El_t);
-    // Transition cv::Matrix
-    m_linearKalman.transitionMatrix = (cv::Mat_<track_t>(4, 4) <<
-                                        1, 0, m_deltaTime, 0,
-                                        0, 1, 0, m_deltaTime,
-                                        0, 0, 1, 0,
-                                        0, 0, 0, 1);
-
-    // init...
-    m_lastPointResult = xy0;
-    m_linearKalman.statePre.at<track_t>(0) = xy0.x;  // x
-    m_linearKalman.statePre.at<track_t>(1) = xy0.y;  // y
-    m_linearKalman.statePre.at<track_t>(2) = xyv0.x; // vx
-    m_linearKalman.statePre.at<track_t>(3) = xyv0.y; // vy
-
-    m_linearKalman.statePost.at<track_t>(0) = xy0.x;
-    m_linearKalman.statePost.at<track_t>(1) = xy0.y;
-    m_linearKalman.statePost.at<track_t>(2) = xyv0.x;
-    m_linearKalman.statePost.at<track_t>(3) = xyv0.y;
-
-    cv::setIdentity(m_linearKalman.measurementMatrix);
-
-    m_linearKalman.processNoiseCov = (cv::Mat_<track_t>(4, 4) <<
-                                       pow(m_deltaTime,4.0)/4.0	,0						,pow(m_deltaTime,3.0)/2.0		,0,
-                                       0						,pow(m_deltaTime,4.0)/4.0	,0							,pow(m_deltaTime,3.0)/2.0,
-                                       pow(m_deltaTime,3.0)/2.0	,0						,pow(m_deltaTime,2.0)			,0,
-                                       0						,pow(m_deltaTime,3.0)/2.0	,0							,pow(m_deltaTime,2.0));
-
-
-    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::CreateLinear(cv::Rect_<track_t> rect0, Point_t 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.
-
-    // 8 state variables (x, y, vx, vy, width, height, vw, vh), 4 measurements (x, y, width, height)
-    m_linearKalman.init(8, 4, 0, El_t);
-    // Transition cv::Matrix
-    m_linearKalman.transitionMatrix = (cv::Mat_<track_t>(8, 8) <<
-                                        1, 0, 0, 0, m_deltaTime, 0,           0,           0,
-                                        0, 1, 0, 0, 0,           m_deltaTime, 0,           0,
-                                        0, 0, 1, 0, 0,           0,           m_deltaTime, 0,
-                                        0, 0, 0, 1, 0,           0,           0,           m_deltaTime,
-                                        0, 0, 0, 0, 1,           0,           0,           0,
-                                        0, 0, 0, 0, 0,           1,           0,           0,
-                                        0, 0, 0, 0, 0,           0,           1,           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.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;
-
-    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>(8, 8) <<
-                                       n1, 0,  0,  0,  n2, 0,  0,  0,
-                                       0,  n1, 0,  0,  0,  n2, 0,  0,
-                                       0,  0,  n1, 0,  0,  0,  n2, 0,
-                                       0,  0,  0,  n1, 0,  0,  0,  n2,
-                                       n2, 0,  0,  0,  n3, 0,  0,  0,
-                                       0,  n2, 0,  0,  0,  n3, 0,  0,
-                                       0,  0,  n2, 0,  0,  0,  n3, 0,
-                                       0,  0,  0,  n2, 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)
-{
-	// 6 state variables, 2 measurements
-	m_linearKalman.init(6, 2, 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>(6, 6) <<
-		1, 0, dt, 0,  dt2, 0,
-		0, 1, 0,  dt, 0,   dt2,
-		0, 0, 1,  0,  dt,  0,
-		0, 0, 0,  1,  0,   dt,
-	    0, 0, 0,  0,  1,   0,
-	    0, 0, 0,  0,  0,   1);
-
-	// init...
-	m_lastPointResult = xy0;
-	m_linearKalman.statePre.at<track_t>(0) = xy0.x;  // x
-	m_linearKalman.statePre.at<track_t>(1) = xy0.y;  // y
-	m_linearKalman.statePre.at<track_t>(2) = xyv0.x; // vx
-	m_linearKalman.statePre.at<track_t>(3) = xyv0.y; // vy
-	m_linearKalman.statePre.at<track_t>(4) = 0;      // ax
-	m_linearKalman.statePre.at<track_t>(5) = 0;      // ay
-
-	m_linearKalman.statePost.at<track_t>(0) = xy0.x;
-	m_linearKalman.statePost.at<track_t>(1) = xy0.y;
-	m_linearKalman.statePost.at<track_t>(2) = xyv0.x;
-	m_linearKalman.statePost.at<track_t>(3) = xyv0.y;
-	m_linearKalman.statePost.at<track_t>(4) = 0;
-	m_linearKalman.statePost.at<track_t>(5) = 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>(6, 6) <<
-		n1, 0, n2, 0, n2, 0,
-		0, n1, 0, n2, 0, n2,
-		n2, 0, n3, 0, n3, 0,
-		0, n2, 0, n3, 0, n3,
-		0, 0, n2, 0, n3, 0,
-		0, 0, 0, n2, 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(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;
-}
-
-#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()
-{
-    if (m_initialized)
-    {
-        cv::Mat prediction;
-
-        switch (m_type)
-        {
-        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));
-    }
-    else
-    {
-
-    }
-    return m_lastPointResult;
-}
-
-//---------------------------------------------------------------------------
-Point_t TKalmanFilter::Update(Point_t pt, bool dataCorrect)
-{
-    if (!m_initialized)
-    {
-        if (m_initialPoints.size() < MIN_INIT_VALS)
-        {
-            if (dataCorrect)
-            {
-                m_initialPoints.push_back(pt);
-                m_lastPointResult = pt;
-            }
-        }
-        if (m_initialPoints.size() == MIN_INIT_VALS)
-        {
-            track_t kx = 0;
-            track_t bx = 0;
-            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);
-
-            switch (m_type)
-            {
-            case tracking::KalmanLinear:
-                if (m_useAcceleration)
-					CreateLinearAcceleration(xy0, xyv0);
-				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;
-        }
-    }
-
-    if (m_initialized)
-    {
-        cv::Mat measurement(2, 1, Mat_t(1));
-        if (!dataCorrect)
-        {
-            measurement.at<track_t>(0) = m_lastPointResult.x;  //update using prediction
-            measurement.at<track_t>(1) = m_lastPointResult.y;
-        }
-        else
-        {
-            measurement.at<track_t>(0) = pt.x;  //update using measurements
-            measurement.at<track_t>(1) = pt.y;
-        }
-        // Correction
-        cv::Mat estimated;
-        switch (m_type)
-        {
-        case tracking::KalmanLinear:
-        {
-            estimated = m_linearKalman.correct(measurement);
-
-            // Inertia correction
-			if (!m_useAcceleration)
-			{
-				track_t currDist = sqrtf(sqr(estimated.at<track_t>(0) - pt.x) + sqr(estimated.at<track_t>(1) - pt.y));
-				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, 2) = m_deltaTime;
-				m_linearKalman.transitionMatrix.at<track_t>(1, 3) = m_deltaTime;
-			}
-            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
-        m_lastPointResult.y = estimated.at<track_t>(1);
-    }
-    else
-    {
-        if (dataCorrect)
-        {
-            m_lastPointResult = pt;
-        }
-    }
-    return m_lastPointResult;
-}
-
-//---------------------------------------------------------------------------
-cv::Rect TKalmanFilter::GetRectPrediction()
-{
-    if (m_initialized)
-    {
-        cv::Mat prediction;
-
-        switch (m_type)
-        {
-        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));
-    }
-    else
-    {
-
-    }
-    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));
-}
-
-//---------------------------------------------------------------------------
-cv::Rect TKalmanFilter::Update(cv::Rect rect, bool dataCorrect)
-{
-    if (!m_initialized)
-    {
-        if (m_initialRects.size() < MIN_INIT_VALS)
-        {
-            if (dataCorrect)
-            {
-                m_initialRects.push_back(rect);
-                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);
-            }
-        }
-        if (m_initialRects.size() == MIN_INIT_VALS)
-        {
-            std::vector<Point_t> initialPoints;
-            Point_t averageSize(0, 0);
-            for (const auto& r : m_initialRects)
-            {
-                initialPoints.emplace_back(static_cast<track_t>(r.x), static_cast<track_t>(r.y));
-                averageSize.x += r.width;
-                averageSize.y += r.height;
-            }
-            averageSize.x /= MIN_INIT_VALS;
-            averageSize.y /= 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);
-            cv::Rect_<track_t> rect0(kx * (MIN_INIT_VALS - 1) + bx, ky * (MIN_INIT_VALS - 1) + by, averageSize.x, averageSize.y);
-            Point_t rectv0(kx, ky);
-
-            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
-                break;
-            }
-        }
-    }
-
-    if (m_initialized)
-    {
-        cv::Mat measurement(4, 1, Mat_t(1));
-        if (!dataCorrect)
-        {
-            measurement.at<track_t>(0) = m_lastRectResult.x;  // update using prediction
-            measurement.at<track_t>(1) = m_lastRectResult.y;
-            measurement.at<track_t>(2) = m_lastRectResult.width;
-            measurement.at<track_t>(3) = m_lastRectResult.height;
-        }
-        else
-        {
-            measurement.at<track_t>(0) = static_cast<track_t>(rect.x);  // update using measurements
-            measurement.at<track_t>(1) = static_cast<track_t>(rect.y);
-            measurement.at<track_t>(2) = static_cast<track_t>(rect.width);
-            measurement.at<track_t>(3) = static_cast<track_t>(rect.height);
-        }
-        // Correction
-        cv::Mat estimated;
-        switch (m_type)
-        {
-        case tracking::KalmanLinear:
-        {
-            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);
-
-            // 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;
-        }
-
-        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);
-        }
-    }
-    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));
-}
-
-//---------------------------------------------------------------------------
-cv::Vec<track_t, 2> TKalmanFilter::GetVelocity() const
-{
-    cv::Vec<track_t, 2> res(0, 0);
-    if (m_initialized)
-    {
-        switch (m_type)
-        {
-        case tracking::KalmanLinear:
-        {
-            if (m_linearKalman.statePre.rows > 3)
-            {
-                int indX = 2;
-                int indY = 3;
-                if (m_linearKalman.statePre.rows > 4)
-                {
-                    indX = 4;
-                    indY = 5;
-                }
-                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/radar_camer_fusion/Tracker/Kalman.h

@@ -1,127 +0,0 @@
-#pragma once
-#include "defines.h"
-#include <memory>
-#include <deque>
-
-#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/
-///
-class TKalmanFilter
-{
-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::Rect GetRectPrediction();
-    cv::Rect Update(cv::Rect 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;
-
-    Point_t m_lastPointResult;
-    cv::Rect_<track_t> m_lastRectResult;
-    cv::Rect_<track_t> m_lastRect;
-
-    bool m_initialized = false;
-    track_t m_deltaTime = 0.2f;
-    track_t m_deltaTimeMin = 0.2f;
-    track_t m_deltaTimeMax = 2 * 0.2f;
-    track_t m_lastDist = 0;
-    track_t m_deltaStep = 0;
-    static const int m_deltaStepsCount = 20;
-    track_t m_accelNoiseMag = 0.5f;
-	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::Rect_<track_t> rect0, Point_t 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
-};
-
-//---------------------------------------------------------------------------
-///
-/// \brief sqr
-/// \param val
-/// \return
-///
-template<class T> inline
-T sqr(T val)
-{
-    return val * val;
-}
-
-///
-/// \brief get_lin_regress_params
-/// \param in_data
-/// \param start_pos
-/// \param in_data_size
-/// \param kx
-/// \param bx
-/// \param ky
-/// \param by
-///
-template<typename T, typename CONT>
-void get_lin_regress_params(
-        const CONT& in_data,
-        size_t start_pos,
-        size_t in_data_size,
-        T& kx, T& bx, T& ky, T& by)
-{
-    T m1(0.), m2(0.);
-    T m3_x(0.), m4_x(0.);
-    T m3_y(0.), m4_y(0.);
-
-    const T el_count = static_cast<T>(in_data_size - start_pos);
-    for (size_t i = start_pos; i < in_data_size; ++i)
-    {
-        m1 += i;
-        m2 += sqr(i);
-
-        m3_x += in_data[i].x;
-        m4_x += i * in_data[i].x;
-
-        m3_y += in_data[i].y;
-        m4_y += i * in_data[i].y;
-    }
-    T det_1 = 1 / (el_count * m2 - sqr(m1));
-
-    m1 *= -1;
-
-    kx = det_1 * (m1 * m3_x + el_count * m4_x);
-    bx = det_1 * (m2 * m3_x + m1 * m4_x);
-
-    ky = det_1 * (m1 * m3_y + el_count * m4_y);
-    by = det_1 * (m2 * m3_y + m1 * m4_y);
-}

src/fusion/radar_camer_fusion/Tracker/ShortPathCalculator.h

@@ -1,65 +0,0 @@
-#pragma once
-#include "defines.h"
-#include "HungarianAlg.h"
-
-///
-/// \brief The SPSettings struct
-///
-struct SPSettings
-{
-    track_t m_distThres = 0.8f;
-    size_t m_maxHistory = 10;
-};
-
-///
-/// \brief The ShortPathCalculator class
-///
-class ShortPathCalculator
-{
-public:
-    ShortPathCalculator(const SPSettings& settings)
-        : m_settings(settings)
-    {
-    }
-    virtual ~ShortPathCalculator()
-    {
-    }
-
-    virtual void Solve(const distMatrix_t& costMatrix, size_t N, size_t M, assignments_t& assignment, track_t maxCost) = 0;
-protected:
-    SPSettings m_settings;
-};
-
-///
-/// \brief The SPHungrian class
-///
-class SPHungrian : public ShortPathCalculator
-{
-public:
-    SPHungrian(const SPSettings& settings)
-        : ShortPathCalculator(settings)
-    {
-    }
-
-    void Solve(const distMatrix_t& costMatrix, size_t N, size_t M, assignments_t& assignment, track_t /*maxCost*/)
-    {
-        m_solver.Solve(costMatrix, N, M, assignment, AssignmentProblemSolver::optimal);
-    }
-
-private:
-    AssignmentProblemSolver m_solver;
-};
-
-///
-/// \brief The SPBipart class
-///
-//class SPBipart : public ShortPathCalculator
-//{
-//public:
-//    SPBipart(const SPSettings& settings)
-//        : ShortPathCalculator(settings)
-//    {
-//    }
-
-//    void Solve(const distMatrix_t& costMatrix, size_t N, size_t M, assignments_t& assignment, track_t maxCost);
-//};

src/fusion/radar_camer_fusion/Tracker/Tracking.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/radar_camer_fusion/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/radar_camer_fusion/Tracker/defines.h

@@ -1,171 +0,0 @@
-#pragma once
-
-#include <vector>
-#include <string>
-#include <map>
-#include <opencv2/opencv.hpp>
-
-
-// ---------------------------------------------------------------------------
-//
-// ---------------------------------------------------------------------------
-typedef float track_t;
-typedef cv::Point_<track_t> Point_t;
-#define El_t CV_32F
-#define Mat_t CV_32FC
-
-typedef std::vector<int> assignments_t;
-typedef std::vector<track_t> distMatrix_t;
-
-///
-/// \brief config_t
-///
-typedef std::multimap<std::string, std::string> config_t;
-
-///
-/// \brief The CRegion class
-///
-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)
-    {
-        R2BRect();
-    }
-
-    CRegion(const cv::Rect& rect, const std::string& type, float confidence)
-        : m_brect(rect), m_type(type), m_confidence(confidence)
-    {
-        B2RRect();
-    }
-
-    CRegion(const cv::RotatedRect& rrect, const std::string& type, float confidence)
-        : m_rrect(rrect), m_type(type), m_confidence(confidence)
-    {
-        R2BRect();
-    }
-    cv::RotatedRect m_rrect;
-    cv::Rect m_brect;
-
-    std::string m_type;
-    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()
-    {
-        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());
-        return m_rrect;
-    }
-};
-
-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
-///
-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]
-	DistsCount
-};
-
-///
-/// \brief The FilterGoal enum
-///
-enum FilterGoal
-{
-    FilterCenter,
-    FilterRect
-};
-
-///
-/// \brief The KalmanType enum
-///
-enum KalmanType
-{
-    KalmanLinear,
-    KalmanUnscented,
-    KalmanAugmentedUnscented
-};
-
-///
-/// \brief The MatchType enum
-///
-enum MatchType
-{
-    MatchHungrian/*,
-    //MatchBipart*/
-};
-
-///
-/// \brief The LostTrackType enum
-///
-enum LostTrackType
-{
-    TrackNone,
-    TrackKCF,
-    TrackMIL,
-    TrackMedianFlow,
-    TrackGOTURN,
-    TrackMOSSE,
-    TrackCSRT/*,
-    TrackDAT,
-    TrackSTAPLE,
-    TrackLDES*/
-};
-}

src/fusion/radar_camer_fusion/Tracker/track.cpp

@@ -1,855 +0,0 @@
-#include "track.h"
-
-//#include "dat/dat_tracker.hpp"
-
-///
-/// \brief CTrack
-/// \param pt
-/// \param region
-/// \param deltaTime
-/// \param accelNoiseMag
-/// \param trackID
-/// \param filterObjectSize
-/// \param externalTrackerForLost
-///
-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::LostTrackType externalTrackerForLost
-        )
-    :
-      m_trackID(trackID),
-      m_regionID(regionID),
-      m_skippedFrames(0),
-      m_lastRegion(region),
-      m_predictionPoint(region.m_rrect.center),
-      m_predictionRect(region.m_rrect),
-      m_kalman(kalmanType, useAcceleration, deltaTime, accelNoiseMag),
-      m_filterObjectSize(filterObjectSize),
-      m_outOfTheFrame(false),
-      m_externalTrackerForLost(externalTrackerForLost)
-{
-    if (filterObjectSize)
-        m_kalman.Update(region.m_brect, true);
-    else
-        m_kalman.Update(m_predictionPoint, true);
-
-    m_trace.push_back(m_predictionPoint, m_predictionPoint);
-}
-
-///
-/// \brief CTrack::CalcDistCenter
-/// \param reg
-/// \return
-///
-track_t CTrack::CalcDistCenter(const CRegion& reg) const
-{
-    Point_t diff = m_predictionPoint - reg.m_rrect.center;
-    return sqrtf(sqr(diff.x) + sqr(diff.y));
-}
-
-///
-/// \brief CTrack::CalcDistRect
-/// \param reg
-/// \return
-///
-track_t CTrack::CalcDistRect(const CRegion& reg) const
-{
-    std::array<track_t, 5> diff;
-    diff[0] = reg.m_rrect.center.x - m_lastRegion.m_rrect.center.x;
-    diff[1] = reg.m_rrect.center.y - m_lastRegion.m_rrect.center.y;
-    diff[2] = static_cast<track_t>(m_lastRegion.m_rrect.size.width - reg.m_rrect.size.width);
-    diff[3] = static_cast<track_t>(m_lastRegion.m_rrect.size.height - reg.m_rrect.size.height);
-    diff[4] = static_cast<track_t>(m_lastRegion.m_rrect.angle - reg.m_rrect.angle);
-
-    track_t dist = 0;
-    for (size_t i = 0; i < diff.size(); ++i)
-    {
-        dist += sqr(diff[i]);
-    }
-    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
-/// \param reg
-/// \return
-///
-track_t CTrack::CalcDistHist(const CRegion& reg, cv::UMat currFrame) 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::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())
-	{
-#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
-	}
-
-	return res;
-}
-
-///
-/// \brief CTrack::Update
-/// \*param region
-/// \param dataCorrect
-/// \param max_trace_length
-/// \param prevFrame
-/// \param currFrame
-/// \param trajLen
-///
-void CTrack::Update(
-        const CRegion& region,
-        bool dataCorrect,
-        size_t max_trace_length,
-        cv::UMat prevFrame,
-        cv::UMat currFrame,
-        int trajLen
-        )
-{
-    if (m_filterObjectSize) // Kalman filter for object coordinates and size
-        RectUpdate(region, dataCorrect, prevFrame, currFrame);
-    else // Kalman filter only for object center
-        PointUpdate(region.m_rrect.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);
-
-        CheckStatic(trajLen, currFrame, region);
-    }
-    else
-    {
-        m_trace.push_back(m_predictionPoint);
-    }
-
-    if (m_trace.size() > max_trace_length)
-        m_trace.pop_front(m_trace.size() - max_trace_length);
-}
-
-///
-/// \brief CTrack::IsStatic
-/// \return
-///
-bool CTrack::IsStatic() const
-{
-    return m_isStatic;
-}
-
-///
-/// \brief CTrack::IsStaticTimeout
-/// \param framesTime
-/// \return
-///
-bool CTrack::IsStaticTimeout(int framesTime) const
-{
-    return (m_staticFrames > framesTime);
-}
-
-///
-/// \brief CTrack::IsOutOfTheFrame
-/// \return
-///
-bool CTrack::IsOutOfTheFrame() const
-{
-	return m_outOfTheFrame;
-}
-
-///
-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;
-
-			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
-///        If result <= 1 then center of the object is inside ellipse with prediction and velocity
-/// \param pt
-/// \return
-///
-track_t CTrack::IsInsideArea(const Point_t& pt, const cv::RotatedRect& rrect) const
-{
-	Point_t pt_(pt.x - rrect.center.x, pt.y - rrect.center.y);
-	track_t r = sqrtf(sqr(pt_.x) + sqr(pt_.y));
-	track_t t = (r > 1) ? acosf(pt_.x / r) : 0;
-	track_t t_ = t - rrect.angle;
-	Point_t pt_rotated(r * cosf(t_), r * sinf(t_));
-
-	return sqr(pt_rotated.x) / sqr(rrect.size.width) + sqr(pt_rotated.y) / sqr(rrect.size.height);
-}
-
-///
-/// \brief CTrack::WidthDist
-/// \param reg
-/// \return
-///
-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;
-    else
-        return reg.m_rrect.size.width / m_lastRegion.m_rrect.size.width;
-}
-
-///
-/// \brief CTrack::HeightDist
-/// \param reg
-/// \return
-///
-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;
-    else
-        return reg.m_rrect.size.height / m_lastRegion.m_rrect.size.height;
-}
-
-///
-/// \brief CTrack::CheckStatic
-/// \param trajLen
-/// \return
-///
-bool CTrack::CheckStatic(int trajLen, cv::UMat currFrame, const CRegion& region)
-{
-    if (!trajLen || static_cast<int>(m_trace.size()) < trajLen)
-    {
-        m_isStatic = false;
-        m_staticFrames = 0;
-        m_staticFrame = cv::UMat();
-    }
-    else
-    {
-        track_t kx = 0;
-        track_t bx = 0;
-        track_t ky = 0;
-        track_t by = 0;
-        get_lin_regress_params(m_trace, m_trace.size() - trajLen, m_trace.size(), kx, bx, ky, by);
-        track_t speed = sqrt(sqr(kx * trajLen) + sqr(ky * trajLen));
-        const track_t speedThresh = 10;
-        if (speed < speedThresh)
-        {
-            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_staticFrames;
-            m_isStatic = true;
-        }
-        else
-        {
-            m_isStatic = false;
-            m_staticFrames = 0;
-            m_staticFrame = cv::UMat();
-        }
-    }
-
-    return m_isStatic;
-}
-
-///
-/// \brief GetLastRect
-/// \return
-///
-cv::RotatedRect CTrack::GetLastRect() const
-{
-    if (m_filterObjectSize)
-    {
-        return m_predictionRect;
-    }
-    else
-    {
-        return cv::RotatedRect(cv::Point2f(m_predictionPoint.x, m_predictionPoint.y), m_predictionRect.size, m_predictionRect.angle);
-    }
-}
-
-///
-/// \brief CTrack::LastRegion
-/// \return
-///
-const CRegion& CTrack::LastRegion() const
-{
-    return m_lastRegion;
-}
-
-///
-/// \brief CTrack::ConstructObject
-/// \return
-///
-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());
-}
-
-///
-/// \brief CTrack::SkippedFrames
-/// \return
-///
-size_t CTrack::SkippedFrames() const
-{
-    return m_skippedFrames;
-}
-
-///
-/// \brief CTrack::SkippedFrames
-/// \return
-///
-size_t& CTrack::SkippedFrames()
-{
-    return m_skippedFrames;
-}
-
-///
-/// \brief RectUpdate
-/// \param region
-/// \param dataCorrect
-/// \param prevFrame
-/// \param currFrame
-///
-void CTrack::RectUpdate(
-        const CRegion& region,
-        bool dataCorrect,
-        cv::UMat prevFrame,
-        cv::UMat currFrame
-        )
-{
-    m_kalman.GetRectPrediction();
-
-    bool recalcPrediction = true;
-
-    auto Clamp = [](int& v, int& size, int hi) -> int
-    {
-        int res = 0;
-
-        if (size < 1)
-            size = 0;
-
-        if (v < 0)
-        {
-            res = v;
-            v = 0;
-            return res;
-        }
-        else if (v + size > hi - 1)
-        {
-			res = v;
-            v = hi - 1 - size;
-            if (v < 0)
-            {
-                size += v;
-                v = 0;
-            }
-            res -= v;
-            return res;
-        }
-        return res;
-    };
-
-    auto UpdateRRect = [&](cv::Rect prevRect, cv::Rect newRect)
-    {
-        m_predictionRect.center.x += newRect.x - prevRect.x;
-        m_predictionRect.center.y += newRect.y - prevRect.y;
-        m_predictionRect.size.width *= newRect.width / static_cast<float>(prevRect.width);
-        m_predictionRect.size.height *= newRect.height / static_cast<float>(prevRect.height);
-    };
-
-    switch (m_externalTrackerForLost)
-    {
-    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)
-        UpdateRRect(m_predictionRect.boundingRect(), m_kalman.Update(region.m_brect, dataCorrect));
-
-    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;
-
-	//std::cout << "brect = " << brect << ", dx = " << dx << ", dy = " << dy << ", outOfTheFrame = " << m_outOfTheFrame << ", predictionPoint = " << m_predictionPoint << std::endl;
-}
-
-///
-/// \brief CreateExternalTracker
-///
-void CTrack::CreateExternalTracker(int channels)
-{
-    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;
-
-	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;
-
-//    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
-    }
-}
-
-///
-/// \brief PointUpdate
-/// \param pt
-/// \param dataCorrect
-///
-void CTrack::PointUpdate(
-        const Point_t& pt,
-        const cv::Size& newObjSize,
-        bool dataCorrect,
-        const cv::Size& frameSize
-        )
-{
-    m_kalman.GetPointPrediction();
-
-    m_predictionPoint = m_kalman.Update(pt, dataCorrect);
-
-    if (dataCorrect)
-    {
-        const int a1 = 1;
-        const int a2 = 9;
-        m_predictionRect.size.width = (a1 * newObjSize.width + a2 * m_predictionRect.size.width) / (a1 + a2);
-        m_predictionRect.size.height = (a1 * newObjSize.height + a2 * m_predictionRect.size.height) / (a1 + a2);
-    }
-
-    auto Clamp = [](track_t& v, int hi) -> bool
-    {
-        if (v < 0)
-        {
-            v = 0;
-            return true;
-        }
-        else if (hi && v > hi - 1)
-        {
-			v = static_cast<track_t>(hi - 1);
-            return true;
-        }
-        return false;
-    };
-	auto p = m_predictionPoint;
-    m_outOfTheFrame = Clamp(p.x, frameSize.width) || Clamp(p.y, frameSize.height) || (m_predictionRect.size.width < 2) || (m_predictionRect.size.height < 2);
-
-	//std::cout << "predictionRect = " << m_predictionRect.boundingRect() << ", outOfTheFrame = " << m_outOfTheFrame << ", predictionPoint = " << m_predictionPoint << std::endl;
-}

src/fusion/radar_camer_fusion/Tracker/track.h

@@ -1,316 +0,0 @@
-#pragma once
-#include <iostream>
-#include <vector>
-#include <deque>
-#include <memory>
-#include <array>
-
-#ifdef USE_OCV_KCF
-#include <opencv2/tracking.hpp>
-#endif
-
-#include "defines.h"
-#include "Kalman.h"
-#include "VOTTracker.hpp"
-
-// --------------------------------------------------------------------------
-///
-/// \brief The TrajectoryPoint struct
-///
-struct TrajectoryPoint
-{
-    ///
-    /// \brief TrajectoryPoint
-    ///
-    TrajectoryPoint()
-        : m_hasRaw(false)
-    {
-    }
-
-    ///
-    /// \brief TrajectoryPoint
-    /// \param prediction
-    ///
-    TrajectoryPoint(const Point_t& prediction)
-        :
-          m_hasRaw(false),
-          m_prediction(prediction)
-    {
-    }
-
-    ///
-    /// \brief TrajectoryPoint
-    /// \param prediction
-    /// \param raw
-    ///
-    TrajectoryPoint(const Point_t& prediction, const Point_t& raw)
-        :
-          m_hasRaw(true),
-          m_prediction(prediction),
-          m_raw(raw)
-    {
-    }
-
-    bool m_hasRaw = false;
-    Point_t m_prediction;
-    Point_t m_raw;
-};
-
-// --------------------------------------------------------------------------
-///
-/// \brief The Trace class
-///
-class Trace
-{
-public:
-    ///
-    /// \brief operator []
-    /// \param i
-    /// \return
-    ///
-    const Point_t& operator[](size_t i) const
-    {
-        return m_trace[i].m_prediction;
-    }
-
-    ///
-    /// \brief operator []
-    /// \param i
-    /// \return
-    ///
-    Point_t& operator[](size_t i)
-    {
-        return m_trace[i].m_prediction;
-    }
-
-    ///
-    /// \brief at
-    /// \param i
-    /// \return
-    ///
-    const TrajectoryPoint& at(size_t i) const
-    {
-        return m_trace[i];
-    }
-
-    ///
-    /// \brief size
-    /// \return
-    ///
-    size_t size() const
-    {
-        return m_trace.size();
-    }
-
-    ///
-    /// \brief push_back
-    /// \param prediction
-    ///
-    void push_back(const Point_t& prediction)
-    {
-        m_trace.emplace_back(prediction);
-    }
-    void push_back(const Point_t& prediction, const Point_t& raw)
-    {
-        m_trace.emplace_back(prediction, raw);
-    }
-
-    ///
-    /// \brief pop_front
-    /// \param count
-    ///
-    void pop_front(size_t count)
-    {
-        if (count < size())
-            m_trace.erase(m_trace.begin(), m_trace.begin() + count);
-        else
-            m_trace.clear();
-    }
-
-    ///
-    /// \brief GetRawCount
-    /// \param lastPeriod
-    /// \return
-    ///
-    size_t GetRawCount(size_t lastPeriod) const
-    {
-        size_t res = 0;
-
-        size_t i = 0;
-        if (lastPeriod < m_trace.size())
-            i = m_trace.size() - lastPeriod;
-
-        for (; i < m_trace.size(); ++i)
-        {
-            if (m_trace[i].m_hasRaw)
-                ++res;
-        }
-
-        return res;
-    }
-
-private:
-    std::deque<TrajectoryPoint> m_trace;
-};
-
-// --------------------------------------------------------------------------
-///
-/// \brief The TrackingObject class
-///
-struct TrackingObject
-{
-    cv::RotatedRect m_rrect;           // Coordinates
-    Trace m_trace;                     // Trajectory
-    size_t m_ID = 0;                   // Objects ID
-    size_t m_regionID = 0;             // region id <M
-    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
-
-    ///
-    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)
-        :
-          m_rrect(rrect), m_ID(ID), m_regionID(regionID), m_isStatic(isStatic), m_outOfTheFrame(outOfTheFrame), m_type(type), m_confidence(confidence), m_velocity(velocity)
-    {
-        for (size_t i = 0; i < trace.size(); ++i)
-        {
-            auto tp = trace.at(i);
-            if (tp.m_hasRaw)
-                m_trace.push_back(tp.m_prediction, tp.m_raw);
-            else
-                m_trace.push_back(tp.m_prediction);
-        }
-    }
-
-    ///
-    bool IsRobust(int minTraceSize, float minRawRatio, cv::Size2f sizeRatio) const
-    {
-        bool res = m_trace.size() > static_cast<size_t>(minTraceSize);
-        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;
-            if (sizeRatio.width > 0)
-                res &= (sr > sizeRatio.width);
-
-            if (sizeRatio.height > 0)
-                res &= (sr < sizeRatio.height);
-        }
-        if (m_outOfTheFrame)
-            res = false;
-
-        return res;
-    }
-};
-
-// --------------------------------------------------------------------------
-///
-/// \brief The CTrack class
-///
-class CTrack
-{
-public:
-    CTrack(const CRegion& region,
-           tracking::KalmanType kalmanType,
-           track_t deltaTime,
-           track_t accelNoiseMag,
-           bool useAcceleration,
-           size_t trackID,
-           size_t regionID,
-           bool filterObjectSize,
-           tracking::LostTrackType externalTrackerForLost);
-
-    ///
-    /// \brief CalcDist
-    /// Euclidean distance in pixels between objects centres on two N and N+1 frames
-    /// \param reg
-    /// \return
-    ///
-    track_t CalcDistCenter(const CRegion& reg) const;
-    ///
-    /// \brief CalcDist
-    /// Euclidean distance in pixels between object contours on two N and N+1 frames
-    /// \param reg
-    /// \return
-    ///
-    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
-    /// \param reg
-    /// \param currFrame
-    /// \return
-    ///
-    track_t CalcDistHist(const CRegion& reg, cv::UMat currFrame) const;
-
-    cv::RotatedRect CalcPredictionEllipse(cv::Size_<track_t> minRadius) const;
-    ///
-    /// \brief IsInsideArea
-    /// Test point inside in prediction area: prediction area + object velocity
-    /// \param pt
-    /// \param minVal
-    /// \return
-    ///
-    track_t IsInsideArea(const Point_t& pt, const cv::RotatedRect& rrect) const;
-    track_t WidthDist(const CRegion& reg) const;
-    track_t HeightDist(const CRegion& reg) const;
-
-    void Update(const CRegion& region, bool dataCorrect, size_t max_trace_length, cv::UMat prevFrame, cv::UMat currFrame, int trajLen);
-
-    bool IsStatic() const;
-    bool IsStaticTimeout(int framesTime) const;
-    bool IsOutOfTheFrame() const;
-
-    cv::RotatedRect GetLastRect() const;
-
-    const Point_t& AveragePoint() const;
-    Point_t& AveragePoint();
-    const CRegion& LastRegion() const;
-    size_t SkippedFrames() const;
-    size_t& SkippedFrames();
-
-    TrackingObject ConstructObject() const;
-public:
-    size_t m_regionID = 0;
-private:
-    Trace m_trace;
-    size_t m_trackID = 0;
-    size_t m_skippedFrames = 0;
-    CRegion m_lastRegion;
-
-    Point_t m_predictionPoint;
-    cv::RotatedRect m_predictionRect;
-    TKalmanFilter m_kalman;
-    bool m_filterObjectSize = false;
-    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);
-
-    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;
-};
-
-typedef std::vector<std::unique_ptr<CTrack>> tracks_t;

src/fusion/radar_camer_fusion/fusion.cpp

@@ -1,353 +0,0 @@
-#include "eigen3/Eigen/Eigen"
-#include "fusion.h"
-#include <iostream>
-
-using namespace Eigen;
-using namespace std;
-
-//计算图像目标在世界坐标系下的坐标，输入图像坐标系坐标，输出世界坐标系坐标
-//float ComputeDis()
-//{
-
-//}
-
-//Point iv::fusion::Objectfusion::CameraToWorld(Point CameraInfo)
-//{
-//    Eigen::Matrix<float, 3,3> M_mat;
-//    M_mat <<
-
-//             -1.779425393883217, 0.08238080527237265, 1497.921944186997,
-//              -2.753065043204539e-14, -7.331891669241049, 2354.984038719003,
-//              -3.021031881519582e-18, -0.001853568118628354, 1.;
-
-
-////             -2.7341490227041249e-01, -1.4344331689120825e-02,2.4706078228937577e+02,
-////             9.3772377361721504e-02,-6.2813369063524449e-01, -3.2874944247737361e+01,
-////             1.0414363407561230e-04, -2.1564718162646128e-03, 1.;
-
-//    Point camerainworld;
-//    Eigen::MatrixXf camerapoint(3,1);
-//    camerapoint(0,0) = CameraInfo.x;
-//    camerapoint(1,0) = CameraInfo.y;
-//    camerapoint(2,0) = 1;
-
-//    MatrixXf m2 = M_mat * camerapoint;
-//    assert(m2(2, 0) != 0);
-
-//    camerainworld.x = (int(m2(0, 0) / m2(2, 0)))/1000;
-//    camerainworld.y = (int(m2(1, 0) / m2(2, 0)))/1000;
-
-//    std::cout<<CameraInfo.x<<"@@@@@@@@@@@@@"<<CameraInfo.y<<std::endl;
-
-//    std::cout<<camerainworld.x<<"~~~~~zzzzzzz~~~~~~~~"<<camerainworld.y<<std::endl;
-
-//    return camerainworld;
-//}
-
-cv::Point3f iv::fusion::Objectfusion::ComputeCameratToWord(const iv::vision::Bbox3D& obs)
-{
-   cv::Point3f pointInWord;
-   Eigen::Matrix<float, 4,4> TR_mat;
-   TR_mat <<
-             5.9291132376544176e-01, 7.8826308046136040e-01, 1.6461311652562305e-01, -9.6543865533233912e+00,
-             2.6858663540518707e-01,-3.8629650733670828e-01, 8.8240366482760124e-01,6.4911669006353305e+00,
-             7.5915570302305757e-01, -4.7897424189723109e-01,-4.4075650212621942e-01,2.6308605270778315e+01,
-             0,0,0,1;
-   Eigen::Matrix<float, 4,1> point_camera_word;
-   point_camera_word << obs.x(),obs.y(),obs.z(),1;
-
-
-   Eigen::Matrix<float,4,1> point_word;
-
-   point_word = TR_mat.transpose()*point_camera_word;
-   pointInWord.x = point_word[0];
-   pointInWord.y = point_word[1];
-   pointInWord.z = point_word[2];
-
-   return pointInWord;
-}
-
-
-
-
-
-//计算毫米波目标距离
-float iv::fusion::Objectfusion::ComputelonDistance(double x, double y)
-{
-    return (sqrt(pow(x,2) + pow(y,2)));
-
-}
-
-//计算毫米波目标速度
-float iv::fusion::Objectfusion::ComputeRadarSpeed(double vx, double vy)
-{
-    return (sqrt(pow(vx,2) + pow(vy,2)));
-}
-
-//毫米波数据去重
-//将速度相近、目标距离相近、纵向坐标相近的目标状态置为false
-void iv::fusion::Objectfusion::RadarPross(iv::radar::radarobjectarray& radar_object)
-{
-
-    for(int r = 0; r<(radar_object.obj_size()-1);r++)
-    {
-//        std::cout<<"r  :    "<<r<<"      size     :"<<radar_object.obj_size()<<std::endl;
-
-
-        if(radar_object.obj(r).bvalid() == false) continue;
-
-        if(radar_object.obj(r).y()>80||(fabs(radar_object.obj(r).x())>15))
-        {
-//            std::cout<<"RadarPross   begin: "<<std::endl;
-            iv::radar::radarobject & radar_objct_ = (iv::radar::radarobject&)radar_object.obj(r);
-            radar_objct_.set_bvalid(false);
-//            radar_object.mutable_obj(r)->set_bvalid(false);
-
-//            std::cout<<"RadarPross   end: "<<std::endl;
-        }
-
-        for (int s=(r+1); s<(radar_object.obj_size());s++)
-        {
-
-            if(radar_object.obj(s).bvalid() == false) continue;
-            float lonDistance1 = ComputelonDistance(radar_object.obj(r).x(),radar_object.obj(r).y());
-            float lonDistance2 = ComputelonDistance(radar_object.obj(s).x(),radar_object.obj(s).y());
-            float RadarSpeed1 = ComputeRadarSpeed(radar_object.obj(r).vx(),radar_object.obj(r).vy());
-            float RadarSpeeds = ComputeRadarSpeed(radar_object.obj(s).vx(), radar_object.obj(s).vy());
-//            std::cout<<"datafusion begin:"<<std::endl;
-            if(
-
-                   (fabs(radar_object.obj(r).x() - radar_object.obj(s).x()) <=0.5)||
-                    ((fabs(lonDistance1- lonDistance2) <= 0.50)&&(fabs(RadarSpeed1-RadarSpeeds)<=0.50))
-             ){
-//                radar_object.mutable_obj(s)->set_bvalid(false);
-                iv::radar::radarobject & radar_objct_new = (iv::radar::radarobject&)radar_object.obj(s);
-                radar_objct_new.set_bvalid(false);
-            }
-        }
-
-    }
-
-
-}
-
-//将毫米波目标投射到图像坐标系上
-//输入毫米波坐标系下坐标，输出图像坐标系下毫米波坐标
-//void iv::fusion::Objectfusion::RadarToCamera(Point& radarinfo, Point& pointIncamera){
-
-////    Eigen::Matrix<double, 3,4> K_mat;
-
-
-//    Eigen::Matrix<double, 3,4> K_mat;
-
-//    Eigen::Matrix<double ,4,4> Trans_mat;
-//    Eigen::Matrix<double, 1,5> distCoeffs;
-
-//    K_mat <<
-//             1053.864895, 0.000000, 971.348303,0.,
-//             0.000000, 968.877713, 550.964881,0.,
-//             0.000000, 0.000000, 1.000000,0.;
-
-//    distCoeffs<<-0.290790, 0.060994, 0.003649, -0.002059, 0.000000;
-
-
-//    Trans_mat <<
-//                 9.9729820313151096e-01,   -7.1503475479664416e-02,  1.6838854622221750e-02,     2.5119309561071688e+01,
-//                 1.3199346311828775e-02,   -5.1070628904529507e-02,  -9.9860781497053985e-01,    1.5333369174382897e+03,
-//                 7.2263900307136072e-02,   9.9613204137685718e-01,   -4.9988847303928038e-02,    -2.4780658279687159e+03,
-//            0.,0.,0.,1.;
-
-//    Eigen::Matrix<double, 4,1> real_position;
-//    real_position<< (radarinfo.x*1000), (radarinfo.y*1000), (radarinfo.z*1000), 1;
-
-//    Eigen::Matrix<double,3,1> uv_mat =K_mat*(Trans_mat*real_position);
-
-//    pointIncamera.x = int(fabs(uv_mat[0]/uv_mat[2]));
-//    pointIncamera.y = int(fabs(uv_mat[1]/uv_mat[2]));
-
-
-//}
-void iv::fusion::Objectfusion::RadarToCamera(std::vector<cv::Point3f>& radarpoint, std::vector<cv::Point2f>& camerapoint)
-{
-
-    cv::Mat K_mat = (cv::Mat_<double>(3,3)<<1.3045004012660161e+03, 0., 5.8482348464526285e+02, 0.,
-                     1.2112526100577470e+03, 1.8679663217256709e+02, 0., 0., 1. );
-    cv::Mat r_mat = (cv::Mat_<double>(3,1)<<-1.9340161235486149e+00, -8.4462583294643478e-01,
-                     -7.3826864590818786e-01 );
-
-    cv::Mat t_mat  = (cv::Mat_<double>(3,1)<<-9.6543865533233912e+00, 6.4911669006353305e+00,
-                      2.6308605270778315e+01);
-    cv::Mat distCoeffs = (cv::Mat_<double>(1,14)<<-9.6729350187520047e-02, -2.1731738003015462e+00, 0., 0., 0.,
-                          0., 0., -4.7445499728170173e+00, 0., 0., 0., 0., 0., 0.);
-
-    cv::projectPoints(radarpoint,r_mat,t_mat,K_mat,distCoeffs,camerapoint);
-
-}
-
-// 计算毫米波目标是否在bbox里面，是返回1,否返回0
-
-int iv::fusion::Objectfusion::Computeradartobox(cv::Point2f radarincamera,Point top_left,Point bot_right)
-{
-    if((radarincamera.x>=(top_left.x))&&(radarincamera.x<=(bot_right.x+5))&&(radarincamera.y<=(bot_right.y+5))&&(radarincamera.y>=(top_left.y)))
-    {
-        return 0;
-    } else {
-        return 1;
-    }
-}
-
-
-
-int iv::fusion::Objectfusion::Computefusionprobabilities(const  iv::radar::radarobject& radar_object, const  iv::vision::Bbox3D& vision_object )
-{
-//    fusion_state fu;
-    std::vector<cv::Point3f> radar_in_word;
-    std::vector<cv::Point2f> radar_in_camera;
-
-    double z = 0.3;
-    int flag = 0;
-    Point top_left, bot_right;
-    top_left.x = vision_object.top_left_x();
-    top_left.y = vision_object.top_left_y();
-    bot_right.x = vision_object.bottom_right_x();
-    bot_right.y = vision_object.bottom_right_y();
-
-    cv::Point3f radarobj;
-    radarobj.x = radar_object.x();
-    radarobj.y = radar_object.y();
-    radarobj.z = 0.5;
-    radar_in_word.push_back(radarobj);
-
-    RadarToCamera(radar_in_word,radar_in_camera);
-
-//    std::cout<<"-------------------------------------------------------"<<std::endl;
-//    std::cout<<"x1  :"<<radar_in_word[0].x<<"   y1 : "<<radar_in_word[0].y<< "    z1 : "<<radar_in_word[0].z<<std::endl;
-
-//    std::cout<<"x2  :"<<radar_in_camera[0].x<<"  y2 :" <<radar_in_camera[0].y<<std::endl;
-
-
-    int result = Computeradartobox(radar_in_camera[0], top_left, bot_right);
-    radar_in_word.clear();
-    radar_in_camera.clear();
-//    std::cout<<camera_object.w()<<std::endl;
-//    std::cout<<result<<"ooooooooooooo"<<std::endl;
-    return result;
-}
-
-//计算毫米波目标距离
-float iv::fusion::Objectfusion::ComputeDis(cv::Point2f po1, cv::Point2f po2)
-{
-    return (sqrt(pow((po1.x-po2.x),2) + pow((po1.y-po2.y),2)));
-}
-
-
-
-//将匹配上的目标有效值放入fusion里面
-void iv::fusion::Objectfusion::datafusion( iv::vision::ObstacleInfo& vision_object_array, iv::radar::radarobjectarray& radar_object_array, iv::fusion::fusionobjectarray& Ra_Ca_fusion_object_array)
-{
-    Ra_Ca_fusion_object_array.Clear();
-    for(int i = 0; i<vision_object_array.bbox_3d_size(); i++)
-    {
-        std::vector<int> fusionindex;
-        for(int j = 0; j<radar_object_array.obj_size(); j++)
-        {
-            if(radar_object_array.obj(j).bvalid() == false) continue;
-            int result = iv::fusion::Objectfusion::Computefusionprobabilities(radar_object_array.obj(j),vision_object_array.bbox_3d(i));
-            fusionindex.push_back(j);
-        }
-        if(fusionindex.size()==0)
-        {
-            cv::Point3f point_word;
-            iv::fusion::fusionobject fusion_object;
-            fusion_object.set_id(vision_object_array.bbox_3d(i).class_id());
-            fusion_object.set_type_name(vision_object_array.bbox_3d(i).category());
-//            fusion_object.set_prob(vision_object_array.bbox_3d(i));    //vision置信度未传出
-
-            iv::fusion::PointXYZ centroid;
-            iv::fusion::PointXYZ *centerpoint;
-//            Point pointIncamera, pointInworld;
-
-//            pointIncamera.x = camera_object_array.obj(i).outpointx();
-//            pointIncamera.y = camera_object_array.obj(i).outpointy();
-//            pointInworld = iv::fusion::Objectfusion::CameraToWorld(pointIncamera);
-            point_word = iv::fusion::Objectfusion::ComputeCameratToWord(vision_object_array.bbox_3d(i));
-
-            centroid.set_x(point_word.x);
-            centroid.set_y(point_word.z);
-            centerpoint=fusion_object.mutable_centroid();
-            centerpoint->CopyFrom(centroid);
-//            iv::fusion::VelXY vel_relative;
-//            iv::fusion::VelXY *velrelative;
-//            vel_relative.set_x(vision_object_array.bbox_3d(i));
-//            vel_relative.set_y(camera_object_array.obj(i).speedy());
-
-//            velrelative = fusion_object.mutable_vel_relative();
-//            velrelative->CopyFrom(vel_relative);
-            iv::fusion::Dimension dimension;
-            iv::fusion::Dimension *obj_dimension;
-            dimension.set_x(1);
-            dimension.set_y(1);
-            dimension.set_y(1);
-            obj_dimension = fusion_object.mutable_dimensions();
-            obj_dimension->CopyFrom(dimension);
-
-            iv::fusion::fusionobject *po = Ra_Ca_fusion_object_array.add_obj();
-            po->CopyFrom(fusion_object);
-        }else{
-
-            vector<float> dis;
-            cv::Point3f point_word;
-            point_word = iv::fusion::Objectfusion::ComputeCameratToWord(vision_object_array.bbox_3d(i));
-
-            cv::Point2f po1;
-            po1.x = point_word.x;
-            po1.y = point_word.z;
-            for(vector<int>::iterator d = fusionindex.begin(); d != fusionindex.end(); d++)
-            {
-                cv::Point2f po2;
-                po2.x = radar_object_array.obj(*d).x();
-                po2.y = radar_object_array.obj(*d).y();
-                dis.push_back(ComputeDis(po1,po2));
-            }
-            auto smallest = std::min_element(std::begin(dis), std::end(dis));
-            int index = std::distance(std::begin(dis), smallest);
-            dis.clear();
-
-            iv::fusion::fusionobject fusion_object;
-            fusion_object.set_id(vision_object_array.bbox_3d(i).class_id());
-            fusion_object.set_type_name(vision_object_array.bbox_3d(i).category());
-//            fusion_object.set_prob(vision_object_array.bbox_3d(i));   //vision置信度未传出
-
-            iv::fusion::PointXYZ centroid;
-            iv::fusion::PointXYZ *centerpoint;
-            centroid.set_x(radar_object_array.obj(fusionindex[index]).x());
-            centroid.set_y(radar_object_array.obj(fusionindex[index]).y());
-            centerpoint=fusion_object.mutable_centroid();
-            centerpoint->CopyFrom(centroid);
-//                 std::cout<<radar_object_array.obj(j).x()<<"~~~~~~~~~~"<<radar_object_array.obj(j).y()<<std::endl;
-
-            iv::fusion::Dimension dimension;
-            iv::fusion::Dimension *obj_dimension;
-            dimension.set_x(1);
-            dimension.set_y(1);
-            dimension.set_y(1);
-            obj_dimension = fusion_object.mutable_dimensions();
-            obj_dimension->CopyFrom(dimension);
-
-            iv::fusion::VelXY vel_relative;
-            iv::fusion::VelXY *velrelative;
-            vel_relative.set_x(radar_object_array.obj(fusionindex[index]).vx());
-            vel_relative.set_y(radar_object_array.obj(fusionindex[index]).vy());
-
-            velrelative = fusion_object.mutable_vel_relative();
-            velrelative->CopyFrom(vel_relative);
-
-            iv::fusion::fusionobject *pe = Ra_Ca_fusion_object_array.add_obj();
-            pe->CopyFrom(fusion_object);
-        }
-        fusionindex.clear();
-    }
-}
-
-
-

src/fusion/radar_camer_fusion/fusion.h

@@ -1,65 +0,0 @@
-#ifndef OBJECT_DISTANCE_H
-#define OBJECT_DISTANCE_H
-#include <eigen3/Eigen/Dense>
-#include "fusionobjectarray.pb.h"
-//#include "cameraobjectarray.pb.h"
-#include "obstacles.pb.h"
-#include "radarobjectarray.pb.h"
-#include "opencv2/opencv.hpp"
-#include "math.h"
-using namespace Eigen;
-
-struct Point{
-    float x;
-    float y;
-    float z;
-};
-
-//struct fusion_state{
-//    int s;
-//    float dis;
-//};
-
-namespace iv {
-
-namespace fusion {
-
-
-
-class Objectfusion{
-
-public:
-    Objectfusion(){};
-    ~Objectfusion(){};
-
-    // radar coordinate transport to camera coordinate
-    static void RadarToCamera(std::vector<cv::Point3f>& radarpoint, std::vector<cv::Point2f>& camerapoint);
-
-    static Point CameraToWorld(Point CameraInfo);
-
-    static void RadarPross(iv::radar::radarobjectarray& radar_object);
-
-    static float ComputelonDistance(double x, double y);
-
-    static float ComputeRadarSpeed(double vx, double vy);
-
-    static float ComputeDis(cv::Point2f po1,cv::Point2f po2);
-
-    static  void datafusion( iv::vision::ObstacleInfo& vision_object_array, iv::radar::radarobjectarray& radar_object_array, iv::fusion::fusionobjectarray& Ra_Ca_fusion_object_array);
-
-//    static fusion_state Computefusionprobabilities(const  iv::radar::radarobject& radar_object, const  iv::vision::cameraobject& camera_object );
-
-    static cv::Point3f ComputeCameratToWord(const iv::vision::Bbox3D& obs);
-
-    static int Computefusionprobabilities(const  iv::radar::radarobject& radar_object, const  iv::vision::Bbox3D& camera_object );
-
-    static int Computeradartobox(cv::Point2f radarincamera,Point top_left,Point bot_right);
-
-    };
-    }
-
-}
-
-
-
-#endif // OBJECT_DISTANCE_H

src/fusion/radar_camer_fusion/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/radar_camer_fusion/main.cpp

@@ -1,184 +0,0 @@
-
-#include <QCoreApplication>
-#include <QDateTime>
-#include <iostream>
-#include "modulecomm.h"
-#include "cameraobjectarray.pb.h"
-#include "objectarray.pb.h"
-#include "radarobjectarray.pb.h"
-#include "fusionobjectarray.pb.h"
-#include "rawpic.pb.h"
-#include "fusionobject.pb.h"
-#include <QThread>
-#include <QString>
-#include "opencv2/highgui.hpp"
-#include "opencv2/opencv.hpp"
-#include "imageBuffer.h"
-#include "fusion.h"
-#include <QMutex>
-#include "data_manager.h"
-
-#include "Tracking.h"
-
-#include <opencv2/opencv.hpp>
-#include <opencv2/core/ocl.hpp>
-
-using namespace iv;
-using namespace fusion;
-
-void * gfu = iv::modulecomm::RegisterSend("ra_ca_fusion",10000000,1);
-static QMutex gMutex;
-
-typedef  iv::radar::radarobjectarray RadarDataType;
-typedef  iv::lidar::objectarray LidarDataType;
-//typedef  iv::vision::cameraobjectarray CameraDataType;
-typedef  iv::vision::ObstacleInfo VisionDataType;
-typedef  std::chrono::system_clock::time_point TimeType;
-//typedef  std::function<void(RadarDataType&, LidarDataType&,VisionDataType&)> DataCallback;
-typedef  std::function<void(RadarDataType&,VisionDataType&)> RVCallback;
-void rv_callback(RadarDataType& radarobjv , VisionDataType& visionobjv);
-
-
-QTime gTime;
-using namespace std;
-int gntemp = 0;
-
-
-//iv::vision::cameraobjectarray* cameraobjs = new iv::vision::cameraobjectarray();
-//iv::radar::radarobjectarray* radarobjs = new iv::radar::radarobjectarray();
-iv::fusion::fusionobjectarray fusionobjvec;
-ConsumerProducerQueue<cv::Mat> *imageBuffer = new ConsumerProducerQueue<cv::Mat>(1, true);
-
-TrackerSettings settings;
-CTracker tracker(settings);
-void radarcamerafusion(iv::vision::ObstacleInfo& visionobjvec,iv::radar::radarobjectarray& radarobjvec);
-iv::DataManager data_manager;
-
-
-bool m_isTrackerInitialized = false;
-
-
-
-void Listenesrfront(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-//    std::cout<<"111111"<<std::endl;
-    iv::radar::radarobjectarray radarobj;
-    if(nSize<1)return;
-    if(false == radarobj.ParseFromArray(strdata,nSize))
-    {
-        std::cout<<"PecerptionShow Listenesrfront fail."<<std::endl;
-        return;
-    }
-    else{
-        //std::cout<<"srfront byte size:  "<<radarobjvec.ByteSize()<<std::endl;
-    }
-    gMutex.lock();
-//    for(int i = 0; i<radarobj.obj_size(); i++)
-//    {
-//        std::cout<<"x   :"<<radarobj.obj(i).x()<<"     y   :"<<radarobj.obj(i).y()<<std::endl;
-//    }
-    data_manager.SetRadar(radarobj);
-    gMutex.unlock();
-//    std::cout<<"-----------------------"<<std::endl;
-//    for(int j = 0; j<radarobj.obj_size(); j++)
-//    {
-//        if(!(radarobj.obj(j).bvalid()==true)) continue;
-//        std::cout<<"obj x: "<<radarobj.obj(j).x()<<"    obj y:"<<radarobj.obj(j).y()<<std::endl;
-//    }
-
-}
-
-void Listencamera(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-//    std::cout<<"22222"<<std::endl;
-    if(nSize<1)return;
-    iv::vision::ObstacleInfo visionobj;
-    if(false == visionobj.ParseFromArray(strdata, nSize))
-    {
-        //        std::cout<<"ccccccccccc Listencamera fail."<<std::endl;
-        return;
-    }
-    else{
-        //std::cout<<"camera byte size:  "<<cameraobjvec.ByteSize()<<std::endl;
-    }
-    gMutex.lock();
-    data_manager.SetCamera(visionobj);
-//    std::cout<<"camera byte size:  "<<visionobj.time()<<std::endl;
-//            for(int j = 0; j<visionobj.bbox_3d_size(); j++)
-//            {
-//                std::cout<<"obj x: "<<visionobj.bbox_3d(j).x()<<"    obj y:"<<visionobj.bbox_3d(j).z()<<std::endl;
-//            }
-    gMutex.unlock();
-}
-
-void rv_callback(RadarDataType& radarobjv ,VisionDataType& visionobjv)
-{
-    if((radarobjv.ByteSize() == 0) || (visionobjv.ByteSize() ==0) ) return;
-//    std::cout<<"radar time:  "<<radarobjv.mstime()<<std::endl;
-//    std::cout<<"camera  time:  "<<cameraobjv.mstime()<<std::endl;
-//     for(int i =0; i<radarobjv.obj_size(); i++)
-//     {
-//         std::cout<<"x   :"<<radarobjv.obj(i).x()<<"y   :"<<radarobjv.obj(i).y()<<std::endl;
-
-//     }
-    radarcamerafusion(visionobjv,radarobjv);
-//    gMutex.lock();
-//    cameraobjs->CopyFrom(cameraobjv);
-//    radarobjs->CopyFrom(radarobjv);
-//    gMutex.unlock();
-//    getpic();
-//    show_result();
-}
-
-void radarcamerafusion(iv::vision::ObstacleInfo& visionobjvec,iv::radar::radarobjectarray& radarobjvec)
-{
-   if(visionobjvec.bbox_3d_size() == 0 || radarobjvec.obj_size() ==0) return;
-    gMutex.lock();
-    Objectfusion::RadarPross(radarobjvec);
-    gMutex.unlock();
-    gMutex.lock();
-    std::cout<<"----start fusion----"<<std::endl;
-    Objectfusion::datafusion(visionobjvec,radarobjvec,fusionobjvec);
-    std::cout<<"----start track----"<<fusionobjvec.obj_size()<<std::endl;
-    if (!m_isTrackerInitialized)
-        {
-            m_isTrackerInitialized = InitTracker(tracker);
-            if (!m_isTrackerInitialized)
-            {
-                std::cerr << "Tracker initialize error!!!" << std::endl;
-            }
-        }
-        Tracking(fusionobjvec, tracker);
-        //--------------------------------------------  end tracking  ---------------------------------------------------
-
-
-        for(int j = 0; j<fusionobjvec.obj_size(); j++)
-        {
-            std::cout<<"obj x: "<<fusionobjvec.obj(j).centroid().x()<<"    obj y:"<<fusionobjvec.obj(j).centroid().y()<<std::endl;
-        }
-    gMutex.unlock();
- 
-    std::string out = fusionobjvec.SerializeAsString();
-    iv::modulecomm::ModuleSendMsg(gfu,out.data(),out.length());
-
-    qDebug("lenth is %d",out.length());
-
-}
-
-int main(int argc, char *argv[])
-{
-    QCoreApplication a(argc, argv);
-
-      tracker.setSettings(settings);
-      data_manager.SetRVCallback(rv_callback);
-      data_manager.Ready();
-
-      void *gpa;
-
-  //    gpa = iv::modulecomm::RegisterRecv("camera_detect",Listendetection);
-      gpa = iv::modulecomm::RegisterRecv("radar0",Listenesrfront);
-  //    gpa = iv::modulecomm::RegisterRecv("camera_output",Listencamera);
-      gpa = iv::modulecomm::RegisterRecv("vision_obstacles", Listencamera);
-
-      return a.exec();
-}

src/fusion/radar_camer_fusion/radar_camer_fusion.pro

@@ -1,90 +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
-
-QMAKE_LFLAGS += -no-pie
-
-# You can also make your code fail to compile if you use deprecated APIs.
-# In order to do so, uncomment the following line.
-# You can also select to disable deprecated APIs only up to a certain version of Qt.
-#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
-INCLUDEPATH += \
-Tracker
-
-SOURCES += \
-    fusion.cpp \
-        main.cpp \
-    ../../include/msgtype/radarobjectarray.pb.cc \
-    ../../include/msgtype/radarobject.pb.cc \
-    ../../include/msgtype/fusionobjectarray.pb.cc \
-    ../../include/msgtype/fusionobject.pb.cc \
-    ../../include/msgtype/cameraobjectarray.pb.cc \
-    ../../include/msgtype/cameraobject.pb.cc \
-    ../../include/msgtype/rawpic.pb.cc \ 
-    Tracker/HungarianAlg.cpp \
-    Tracker/Ctracker.cpp \
-    Tracker/Kalman.cpp \
-    Tracker/track.cpp \
-    ../../include/msgtype/areaarray.pb.cc \
-    ../../include/msgtype/obstacles.pb.cc
-
-
-# Default rules for deployment.
-qnx: target.path = /tmp/$${TARGET}/bin
-else: unix:!android: target.path = /opt/$${TARGET}/bin
-!isEmpty(target.path): INSTALLS += target
-
-HEADERS += \
-    ../../include/msgtype/radarobjectarray.pb.h \
-    ../../include/msgtype/radarobject.pb.h \
-    ../../include/msgtype/fusionobjectarray.pb.h \
-    ../../include/msgtype/fusionobject.pb.h \
-    ../../include/msgtype/cameraobjectarray.pb.h \
-    ../../include/msgtype/cameraobject.pb.h \
-    fusion.h \
-    imageBuffer.h \
-    ../../include/msgtype/rawpic.pb.h \
-    Tracker/defines.h \
-    Tracker/HungarianAlg.h \
-    Tracker/Ctracker.h \
-    Tracker/Kalman.h \
-    Tracker/ShortPathCalculator.h \
-    Tracker/track.h \
-    Tracker/VOTTracker.hpp \
-    ../../include/msgtype/obstacles.pb.h
-
-INCLUDEPATH += /usr/include/eigen3
-INCLUDEPATH += $$PWD/../../include/msgtype
-
-
-LIBS += -lprotobuf
-
-#INCLUDEPATH += \
-#$$PWD/../../common/xmlparam \
-#$$PWD/../../common/modulecomm \
-#$$PWD/../../common/ivlog \
-#$$PWD/../../common/ivfault \
-#$$PWD/../../common/Time_Manager_0714/
-#LIBS += \
-#$$PWD/../../common/xmlparam/libxmlparam.so \
-#$$PWD/../../common/modulecomm/libmodulecomm.so \
-#$$PWD/../../common/ivlog/libivlog.so \
-#$$PWD/../../common/ivfault/libivfault.so
-INCLUDEPATH += $$PWD/../../../include/
-
-LIBS += -L$$PWD/../../../bin -lxmlparam -lmodulecomm -livlog -livfault -ldatamanager
-
-#INCLUDEPATH += /usr/include/
-
-LIBS += /usr/lib/libopencv*.so
-
-#INCLUDEPATH += /opt/ros/kinetic/include/opencv-3.3.1-dev
-
-#LIBS += /opt/ros/kinetic/lib/x86_64-linux-gnu/libopencv*.so
-